source: cpp/frams/genetics/fH/fH_general.cpp @ 821

Last change on this file since 821 was 821, checked in by Maciej Komosinski, 5 years ago

Performance and stability improvements in fB, fH, and fL; improved parsing and math evaluations in fL

File size: 27.8 KB
Line 
1// This file is a part of Framsticks SDK.  http://www.framsticks.com/
2// Copyright (C) 1999-2018  Maciej Komosinski and Szymon Ulatowski.
3// See LICENSE.txt for details.
4
5#include <string>
6#include <limits>
7#include <algorithm>
8#include <frams/util/multirange.h>
9#include <utility>
10#include "fH_general.h"
11
12using namespace std;
13#undef max //this macro is not needed here and it clashes with numeric_limits<>::max()
14
15
16
17// Methods for loading handles
18
19const char *fH_part_names[FH_PART_PROPS_COUNT] = { "dn", "fr", "ing", "as" };
20
21const char *fH_joint_names[FH_JOINT_PROPS_COUNT] = { "stif", "rotstif", "stam" };
22
23void fH_Handle::loadProperties(Param par)
24{
25        // loading values for vectors
26        for (int i = 0; i < dimensions; i++)
27        {
28                first[i] = par.getDouble(i);
29                second[i] = par.getDouble(dimensions + i);
30        }
31        obj = par.getSelected();
32}
33
34void fH_Builder::addHandle(fH_Handle *handle)
35{
36        switch (handle->type)
37        {
38        case fHBodyType::JOINT:
39                sticks.push_back((fH_StickHandle*)handle);
40                break;
41        case fHBodyType::NEURON:
42                neurons.push_back((fH_NeuronHandle*)handle);
43                break;
44        case fHBodyType::CONNECTION:
45                connections.push_back((fH_ConnectionHandle*)handle);
46                break;
47        }
48}
49
50// Methods for saving properties of handles in params
51
52void fH_Handle::saveProperties(Param &par)
53{
54        par.select(obj);
55        for (int i = 0; i < dimensions; i++)
56        {
57                par.setDouble(i, first[i]);
58                par.setDouble(dimensions + i, second[i]);
59        }
60}
61
62// Destructor of Builder
63
64fH_Builder::~fH_Builder()
65{
66        for (fH_StickHandle *obj : sticks)
67        {
68                delete obj;
69        }
70        sticks.clear();
71        for (fH_NeuronHandle *obj : neurons)
72        {
73                delete obj;
74        }
75        neurons.clear();
76        for (fH_ConnectionHandle *obj : connections)
77        {
78                delete obj;
79        }
80        connections.clear();
81
82        if (stickparamtab) ParamObject::freeParamTab(stickparamtab);
83        if (neuronparamtab) ParamObject::freeParamTab(neuronparamtab);
84        if (connectionparamtab) ParamObject::freeParamTab(connectionparamtab);
85
86}
87
88// Methods for parsing genotype
89
90void fH_Builder::prepareParams()
91{
92        for (int i = 0; i < dimensions; i++) // preparing first vector fields
93        {
94                string x = "x";
95                x += to_string(i);
96                stickmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);
97                neuronmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);
98                connectionmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);
99
100        }
101        for (int i = 0; i < dimensions; i++) // preparing second vector fields
102        {
103                string y = "y";
104                y += to_string(i);
105                stickmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);
106                neuronmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);
107                connectionmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);
108
109        }
110
111        Part p;
112        for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
113        {
114                stickmut.addProperty(&p.properties().getParamTab()[p.properties().findId(fH_part_names[i]) + p.properties().getGroupCount()], -1);
115        }
116
117        Joint j;
118        for (int i = 0; i < FH_JOINT_PROPS_COUNT; i++)
119        {
120                stickmut.addProperty(&j.properties().getParamTab()[j.properties().findId(fH_joint_names[i]) + j.properties().getGroupCount()], -1);
121        }
122        stickmut.addProperty(NULL, "l", STICKH_LENGTH_TYPE, "length", "", 0, 0, -1);
123
124        Neuro n;
125        neuronmut.addProperty(&n.properties().getParamTab()[n.properties().findId(FH_PE_NEURO_DET) + n.properties().getGroupCount()], -1);
126
127        Param tmp(f0_neuroconn_paramtab, NULL);
128        connectionmut.addProperty(&tmp.getParamTab()[tmp.findId(FH_PE_CONN_WEIGHT) + tmp.getGroupCount()], -1);
129
130        stickparamtab = ParamObject::makeParamTab((ParamInterface *)&stickmut, 0, 0, stickmut.firstMutableIndex());
131        neuronparamtab = ParamObject::makeParamTab((ParamInterface *)&neuronmut, 0, 0, neuronmut.firstMutableIndex());
132        connectionparamtab = ParamObject::makeParamTab((ParamInterface *)&connectionmut, 0, 0, connectionmut.firstMutableIndex());
133}
134
135int fH_Builder::processLine(SString line, int linenumber, int begin, int end)
136{
137        // Firstly, method determines if line describes joint, neuron or neural connection
138        // and prepares corresponding ParamTab
139        fH_Handle *handle = NULL;
140        ParamEntry *tab = NULL;
141        if (line.startsWith("j:")) //joint
142        {
143                handle = new fH_StickHandle(dimensions, begin, end);
144                tab = stickparamtab;
145        }
146        else if (line.startsWith("n:")) //neuron
147        {
148                handle = new fH_NeuronHandle(dimensions, begin, end);
149                tab = neuronparamtab;
150        }
151        else if (line.startsWith("c:")) //connection
152        {
153                handle = new fH_ConnectionHandle(dimensions, begin, end);
154                tab = connectionparamtab;
155        }
156        else // could not determine type of a handle
157        {
158                string message = "Cannot determine handle type at line:  " + to_string(linenumber);
159                logMessage("fH_Builder", "processLine", LOG_ERROR, message.c_str());
160                return begin;
161        }
162        line = line.substr(2); // skip of "j:", "c:" or "n:"
163
164        // Secondly, ParamObject for holding handle properties is created
165        void *obj = ParamObject::makeObject(tab);
166        Param par(tab, obj);
167        par.setDefault();
168        ParamInterface::LoadOptions opts;
169
170        // After preparing Param objects, vector values and body properties are parsed
171        par.load(ParamInterface::FormatSingleLine, line, &opts);
172
173        // If parsing failed, method writes error message and ends processing
174        if (opts.parse_failed)
175        {
176                string message = "Error in parsing handle parameters at line:  " + to_string(linenumber);
177                logMessage("fH_Builder", "processLine", LOG_ERROR, message.c_str());
178                delete handle;
179                ParamObject::freeObject(obj);
180                return begin;
181        }
182
183        // If parsing ended successfully, parsed properties are loaded into handle fields
184        handle->loadProperties(par);
185
186        // In the end, ready handle is stored in an appropriate vector
187        addHandle(handle);
188        return 0;
189}
190
191int fH_Builder::parseGenotype(const SString &genotype)
192{
193        // Firstly, number of dimensions is parsed
194        int pos = 0;
195        SString numdimensions;
196        genotype.getNextToken(pos, numdimensions, '\n');
197        if (!ExtValue::parseInt(numdimensions.c_str(), dimensions, true, false))
198        {
199                logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Could not parse number of dimensions");
200                return 1;
201        }
202        if (dimensions < 1)
203        {
204                logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Number of dimensions cannot be lower than 1");
205                return 1;
206        }
207        SString line;
208        int linenumber = 2;
209
210        // With known number of dimensions ParamTabs for handles are prepared
211        prepareParams();
212
213        // After preparing Builder for parsing, each line is processed with processLine
214        int lastpos = pos;
215        while (genotype.getNextToken(pos, line, '\n'))
216        {
217                if (line.len() > 0)
218                {
219                        int res = processLine(line, linenumber, lastpos, pos - 1);
220                        if (res != 0)
221                        {
222                                return res;
223                        }
224                }
225                lastpos = pos;
226                linenumber++;
227        }
228        if (sticks.size() == 0)
229        {
230                logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Genotype does not contain any stick");
231                return 1;
232        }
233        return 0;
234}
235
236// Distance calculations
237
238double fH_Handle::dist(vector<double> left, vector<double> right)
239{
240        double sum = 0;
241        for (unsigned int i = 0; i < left.size(); i++)
242        {
243                sum += (left[i] - right[i]) * (left[i] - right[i]);
244        }
245        return sqrt(sum);
246}
247
248vector<double> fH_Handle::getVectorsAverage()
249{
250        vector<double> result(dimensions, 0);
251        for (int i = 0; i < dimensions; i++)
252        {
253                result[i] = (first[i] + second[i]) / 2;
254        }
255        return result;
256}
257
258double fH_StickHandle::distance(fH_Handle *right)
259{
260        double distance = 0;
261        switch (right->type)
262        {
263        case fHBodyType::JOINT:
264                // distance is computed between second vector of current handle and first
265                // vector of second handle
266                distance = dist(second, right->first);
267                break;
268        case fHBodyType::NEURON:
269        {
270                // if neuron has to be connected to joint, then distance is calculated
271                // between averages of both handles
272                vector<double> avgs = getVectorsAverage();
273                vector<double> avgn = right->getVectorsAverage();
274                distance = dist(avgs, avgn);
275                break;
276        }
277        case fHBodyType::CONNECTION:
278                // it is impossible to calculate distance between Joint and Connection
279                return numeric_limits<double>::quiet_NaN();
280        }
281        return distance;
282}
283
284double fH_NeuronHandle::distance(fH_Handle *right)
285{
286        double distance = 0;
287        switch (right->type)
288        {
289        case fHBodyType::JOINT:
290        {
291                // if neuron has to be connected to joint, then distance is calculated
292                // between averages of both handles
293                vector<double> avgs = right->getVectorsAverage();
294                vector<double> avgn = getVectorsAverage();
295                distance = dist(avgs, avgn);
296                break;
297        }
298        case fHBodyType::CONNECTION:
299                // this calculation is meant for input neuron - it compares second vector
300                // of neuron and first vector of connection
301                distance = dist(second, right->first);
302                break;
303        case fHBodyType::NEURON:
304                // it is impossible to calculate distance between two Neurons
305                return numeric_limits<double>::quiet_NaN();
306        }
307        return distance;
308}
309
310double fH_NeuronHandle::distance(fH_StickHandle *right, bool first)
311{
312        vector<double> avgn = getVectorsAverage();
313        double distance = 0;
314        if (first)
315        {
316                distance = dist(avgn, right->firstparthandle);
317        }
318        else
319        {
320                distance = dist(avgn, right->secondparthandle);
321        }
322        return distance;
323}
324
325double fH_ConnectionHandle::distance(fH_Handle *right)
326{
327        double distance = 0;
328        switch (right->type)
329        {
330        case fHBodyType::NEURON:
331                // this calculation is meant for output neuron - it compares second vector
332                // of connection and first vector of neuron
333                distance = dist(second, right->first);
334                break;
335        case fHBodyType::JOINT:
336        case fHBodyType::CONNECTION:
337                // it is impossible to calculate distance between Connection and other
338                // Connection or Joint
339                return numeric_limits<double>::quiet_NaN();
340        }
341        return distance;
342}
343
344// Creature build functions
345
346Part * fH_StickHandle::createPart(ParamEntry *tab, std::vector<fH_StickHandle *> *children, Model *model, bool createmapping)
347{
348        Param par(tab, obj);
349        double partprops[FH_PART_PROPS_COUNT];
350        for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
351        {
352                partprops[i] = par.getDouble(2 * getDimensions() + i);
353        }
354
355        unsigned int stickscount = children->size() + 1;
356
357        MultiRange ranges;
358        ranges.add(begin, end);
359
360        for (fH_StickHandle *child : (*children))
361        {
362                par.select(child->obj);
363                for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
364                {
365                        partprops[i] += par.getDouble(2 * getDimensions() + i);
366                }
367                ranges.add(child->begin, child->end);
368        }
369
370        for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
371        {
372                partprops[i] /= stickscount;
373        }
374
375        Part *newpart = new Part();
376
377        model->addPart(newpart);
378
379        newpart->density = partprops[0];
380        newpart->friction = partprops[1];
381        newpart->ingest = partprops[2];
382        newpart->assim = partprops[3];
383
384        if (createmapping) newpart->addMapping(ranges);
385
386        return newpart;
387}
388
389Joint* fH_StickHandle::createJoint(ParamEntry *tab, Model *model, bool createmapping)
390{
391        Param par(tab, obj);
392        if (firstpart == NULL || secondpart == NULL)
393        {
394                return NULL;
395        }
396        Joint *newjoint = new Joint();
397
398        model->addJoint(newjoint);
399
400        newjoint->stif = par.getDoubleById("stif");
401        newjoint->rotstif = par.getDoubleById("rotstif");
402        newjoint->stamina = par.getDoubleById("stam");
403        newjoint->attachToParts(firstpart, secondpart);
404        if (createmapping) newjoint->addMapping(IRange(begin, end));
405        return newjoint;
406}
407
408void fH_Builder::buildBody()
409{
410        // stickconnections vector holds information about connections between sticks.
411        // Left side of pair should hold pointer to stick that is connected with second
412        // vector, and right side of pair should hold pointer to stick that is connected
413        // with first vector
414        stickconnections.clear();
415
416        // if body consists of single stick, just add it to body
417        if (sticks.size() == 1)
418        {
419                stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(NULL, sticks[0]));
420                sticksorder.push_back(0);
421                return;
422        }
423
424        vector<bool> remainingsticks(sticks.size(), true);
425
426        // first we find two handles that have minimal distances between their second
427        // and first vector
428        fH_StickHandle *left = sticks[0];
429        fH_StickHandle *right = sticks[1];
430        double mindist = left->distance(right);
431        int leftid = 0;
432        int rightid = 1;
433        for (unsigned int i = 0; i < sticks.size(); i++)
434        {
435                for (unsigned int j = i + 1; j < sticks.size(); j++)
436                {
437                        double distance = sticks[i]->distance(sticks[j]);
438                        if (distance < mindist)
439                        {
440                                mindist = distance;
441                                left = sticks[i];
442                                right = sticks[j];
443                                leftid = i;
444                                rightid = j;
445                        }
446                        distance = sticks[j]->distance(sticks[i]);
447                        if (distance < mindist)
448                        {
449                                mindist = distance;
450                                left = sticks[j];
451                                right = sticks[i];
452                                leftid = j;
453                                rightid = i;
454                        }
455                }
456        }
457
458        // two found handles are the beginning of creature body
459        stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(NULL, left));
460        stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(left, right));
461
462        // after selecting two handles as beginning of body, they are marked as used
463        // in the list of remaining sticks
464        remainingsticks[leftid] = false;
465        remainingsticks[rightid] = false;
466
467        sticksorder.push_back(leftid);
468        sticksorder.push_back(rightid);
469
470        // next stick is selected by minimum distance between first vector of its handle
471        // and second vector of any existing StickHandle in body
472        int remaining = sticks.size() - 2;
473        while (remaining > 0)
474        {
475                leftid = -1;
476                rightid = -1;
477                mindist = numeric_limits<double>::max();
478                for (unsigned int i = 0; i < sticks.size(); i++)
479                {
480                        // if stick is not already in
481                        if (remainingsticks[i])
482                        {
483                                for (int stickid : sticksorder)
484                                {
485                                        double distance = sticks[stickid]->distance(sticks[i]);
486                                        if (distance < mindist)
487                                        {
488                                                mindist = distance;
489                                                leftid = stickid;
490                                                rightid = i;
491                                        }
492                                }
493                        }
494                }
495                stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(sticks[leftid], sticks[rightid]));
496                remainingsticks[rightid] = false;
497                sticksorder.push_back(rightid);
498                remaining--;
499        }
500}
501
502int fH_Builder::developBrain(Model *model, bool createmapping)
503{
504        Param par(neuronparamtab, NULL);
505        // First of all, neurons are attached to body
506        for (fH_NeuronHandle *currneu : neurons)
507        {
508                par.select(currneu->obj);
509                // create Neuro object and set details
510                currneu->neuron = new Neuro();
511                SString det = par.getStringById("d");
512                if (det != "")
513                {
514                        currneu->neuron->setDetails(det);
515                }
516                else
517                {
518                        currneu->neuron->setDetails("N");
519                }
520
521                // get class of neuron. If class with given name does not exist - return error
522                NeuroClass *nclass = currneu->neuron->getClass();
523                if (!nclass)
524                {
525                        SString msg = "NeuroClass given in details \"";
526                        msg += det + "\" does not exist";
527                        logMessage("fH_Builder", "developBrain", LOG_ERROR, msg.c_str());
528                        delete currneu->neuron;
529                        return -1;
530                }
531                // add neuron to model -> required before attaching to body part
532                model->addNeuro(currneu->neuron);
533                if (nclass->getPreferredLocation() == 2) // attach to Joint
534                {
535                        // find stick that has closest average handle to average handle of
536                        // neuron
537                        double mindist = currneu->distance(sticks[0]);
538                        fH_StickHandle *minstick = sticks[0];
539                        for (unsigned int i = 1; i < sticks.size(); i++)
540                        {
541                                double distance = currneu->distance(sticks[i]);
542                                if (distance < mindist)
543                                {
544                                        mindist = distance;
545                                        minstick = sticks[i];
546                                }
547                        }
548                        currneu->neuron->attachToJoint(minstick->joint);
549                }
550                else if (nclass->getPreferredLocation() == 1) // attach to Part
551                {
552                        // in the beginning we take first part of first stick to calculate
553                        // distance between them as initial minimal distance
554                        double mindist = currneu->distance(sticks[0], true);
555                        Part *minpart = sticks[0]->firstpart;
556                        for (unsigned int i = 0; i < sticks.size(); i++)
557                        {
558                                // after this we take only second parts of following sticks to
559                                // avoid repetition (thats why we start from i = 0)
560                                double distance = currneu->distance(sticks[i], false);
561                                if (distance < mindist)
562                                {
563                                        mindist = distance;
564                                        minpart = sticks[i]->secondpart;
565                                }
566                        }
567                        currneu->neuron->attachToPart(minpart);
568                }
569                if (createmapping) currneu->neuron->addMapping(IRange(currneu->begin, currneu->end));
570                model->checkpoint();
571        }
572
573        par.setParamTab(connectionparamtab);
574        // Secondly, connections are created
575        for (fH_ConnectionHandle *currcon : connections)
576        {
577                par.select(currcon->obj);
578                // Connection is created as follows:
579                //   beginneu ---> endneu
580                // distance between beginneu and connection is calculated as distance
581                // between second handle of beginneu and first handle of connection.
582                // This is why calculation is written as beginneu->distance(currcon).
583                // In case of connection and endneu distance between them is calculated
584                // as distance between second handle of connection and first handle of
585                // endneu. This is why calculation is written as currcon->distance(endneu).
586
587                fH_NeuronHandle *beginneu = NULL;
588                double mindist = numeric_limits<double>::max();
589                // find beginning of connection
590                for (fH_NeuronHandle *neuron : neurons)
591                {
592                        // These method checked earlier if all neurons have valid classes.
593                        // If a neuron does not have output, then it's skipped from comparison.
594                        // Otherwise:
595                        if (neuron->neuron->getClass()->getPreferredOutput() > 0)
596                        {
597                                double distance = neuron->distance(currcon);
598                                if (distance < mindist)
599                                {
600                                        mindist = distance;
601                                        beginneu = neuron;
602                                }
603                        }
604                }
605                // if there was no neuron that could begin connection, then return warning
606                if (!beginneu)
607                {
608                        // due to often appearance of connection genes in fB encoding, this
609                        // log message is commented
610                        // logMessage("fH_Builder", "developBrain", LOG_DEBUG, "There are no available neurons with outputs, connection could not be established");
611                        continue;
612                }
613
614                fH_NeuronHandle *endneu = NULL;
615                mindist = numeric_limits<double>::max();
616                // find ending of connection
617                for (fH_NeuronHandle *neuron : neurons)
618                {
619                        // Method checked earlier if all neurons have valid classes.
620                        // If neuron does not accept input or all inputs are already connected,
621                        // then it's skipped from comparison.
622                        // Otherwise:
623                        if (neuron->neuron->getClass()->getPreferredInputs() == -1 ||
624                                neuron->neuron->getClass()->getPreferredInputs() > neuron->neuron->getInputCount())
625                        {
626                                double distance = currcon->distance(neuron);
627                                if (distance < mindist)
628                                {
629                                        mindist = distance;
630                                        endneu = neuron;
631                                }
632                        }
633                }
634                // if there was no neuron that could end connection, then return warning
635                if (!endneu)
636                {
637                        // due to often appearance of connection genes in fB encoding, this
638                        // log message is commented
639                        // logMessage("fH_Builder", "developBrain", LOG_DEBUG, "There are no available neurons with free inputs, connection could not be established");
640                        continue;
641                }
642                endneu->neuron->addInput(beginneu->neuron, par.getDoubleById("w"));
643                if (createmapping) endneu->neuron->addMapping(IRange(currcon->begin, currcon->end));
644                model->checkpoint();
645        }
646        return 0;
647}
648
649Pt3D fH_Builder::getNextDirection(int count, int number)
650{
651        // In order to get evenly distributed sticks coming from the same Part method
652        // uses algorithm for even distribution of points on a sphere. There are several
653        // methods to perform this, usually they are iterative. This method introduced
654        // below offers not fully accurate, yet quite satisfying results. This is
655        // RSZ method (Rakhmanov, Saff and Zhou method), with use of the golden angle.
656        // This method is based on distribution of points along spiral that covers sphere
657        // surface.
658
659        // Following method works partially on spherical coordinates (r and theta is used).
660        // The Z coordinate is from Cartesian coordinate system. The golden angle is used
661        // to "iterate" along spiral, while Z coordinate is used to move down the
662        // sphere.
663
664        double golden_angle = M_PI * (3.0 - sqrt(5));
665        double dz = 2.0 / (double)count;
666        double z = 1 - ((double)number + 0.5) * dz;
667        double r = sqrt(1 - z * z);
668        double theta = golden_angle * number;
669        Pt3D vec;
670        // In the end X and Y coordinates are calculated with current values of
671        // r and theta. Value z is already calculated
672        vec.x = r * cos(theta);
673        vec.y = r * sin(theta);
674        vec.z = z;
675        vec.normalize();
676        return vec;
677}
678
679Orient fH_Builder::getRotationMatrixToFitVector(Pt3D currdir, Pt3D expecteddir)
680{
681        Orient res;
682        // first method normalizes vectors for easy calculations
683        currdir.normalize();
684        expecteddir.normalize();
685        double c = currdir.dotProduct(expecteddir); // dot product of both vectors
686        // if the dot product of both vectors equals 0
687        if (c == 0)
688        {
689                res.x.x = -1;
690                res.x.y = 0;
691                res.x.z = 0;
692
693                res.y.x = 0;
694                res.y.y = -1;
695                res.y.z = 0;
696
697                res.z.x = 0;
698                res.z.y = 0;
699                res.z.z = -1;
700        }
701        Pt3D v = Pt3D(0); // cross product of both vectors
702        v.x = currdir.y * expecteddir.z - currdir.z * expecteddir.y;
703        v.y = currdir.z * expecteddir.x - currdir.x * expecteddir.z;
704        v.z = currdir.x * expecteddir.y - currdir.y * expecteddir.x;
705
706        // Rotation matrix that enables aligning currdir to expecteddir comes from
707        // following calculation
708        // R = I + [v]_x + ([v]_x)^2 / (1+c)
709        // where [v]_x is the skew-symmetric cross-product matrix of v
710        res.x.x = 1 - (v.y * v.y + v.z * v.z) / (1 + c);
711        res.x.y = v.z + (v.x * v.y) / (1 + c);
712        res.x.z = -v.y + (v.x * v.z) / (1 + c);
713        res.y.x = -v.z + (v.x * v.y) / (1 + c);
714        res.y.y = 1 - (v.x * v.x + v.z * v.z) / (1 + c);
715        res.y.z = v.x + (v.y * v.z) / (1 + c);
716        res.z.x = v.y + (v.x * v.z) / (1 + c);
717        res.z.y = -v.x + (v.y * v.z) / (1 + c);
718        res.z.z = 1 - (v.x * v.x + v.y * v.y) / (1 + c);
719
720        return res;
721}
722
723Model* fH_Builder::buildModel(bool using_checkpoints)
724{
725        Model *model = new Model();
726
727        // At first, floating sticks are connected
728        buildBody();
729
730        model->open(using_checkpoints);
731
732        // Secondly, parts and joints are created
733        // For every stick in body, starting with initial
734        Param par(stickparamtab, NULL);
735        for (int currid : sticksorder)
736        {
737                fH_StickHandle *currstick = sticks[currid];
738                fH_StickHandle *parent = NULL;
739                // find parent of current stick - it is first element of pair, in which
740                // current stick is second
741                for (pair<fH_StickHandle *, fH_StickHandle *> conn : stickconnections)
742                {
743                        if (conn.second == currstick)
744                        {
745                                parent = conn.first;
746                                break;
747                        }
748                }
749
750                // if parent is NULL, then create Part with current stick properties and
751                // location at (0,0,0)
752                if (!parent)
753                {
754                        vector<fH_StickHandle *> emptylist;
755                        Part *firstpart = currstick->createPart(stickparamtab, &emptylist, model, createmapping);
756                        firstpart->p = Pt3D(0);
757                        currstick->firstpart = firstpart;
758                        currstick->firstparthandle = currstick->first; // this is used to calculate later distance between
759                        model->checkpoint();
760                }
761                else //otherwise first part of current stick is the second part of previous stick
762                {
763                        currstick->firstpart = parent->secondpart;
764                        currstick->firstparthandle = parent->secondparthandle;
765                }
766                // position of second part depends on two things
767                //  1. direction of previous joint
768                //  2. how many sticks are connected to the same parent
769                // default direction of growth (without parent) is (1,0,0)
770                Pt3D direction(1, 0, 0);
771                Pt3D secondposition(currstick->firstpart->p);
772                // if parent does exist, then determine how many sticks are connected to
773                // parent and distribute them evenly on a sphere surrounding second part
774                if (parent)
775                {
776                        // improved RSZ method creates vectors that starts in
777                        // center of sphere (which will act as shared part), so direction
778                        // calculated below should point from shared part to previous part
779                        // in order to perform proper aligning
780                        direction = parent->secondpart->p - parent->firstpart->p;
781                        direction.normalize();
782                        // determine how many sticks are connected to parent and when connection
783                        // between parent and current stick appear
784                        int count = 0;
785                        int id = -1;
786                        for (unsigned int i = 0; i < stickconnections.size(); i++)
787                        {
788                                if (stickconnections[i].first == parent)
789                                {
790                                        if (stickconnections[i].second == currstick)
791                                        {
792                                                id = count;
793                                        }
794                                        count++;
795                                }
796                        }
797                        if (id == -1)
798                        {
799                                logMessage("fH_Builder", "buildModel", LOG_ERROR, "Invalid behaviour");
800                                delete model;
801                                return NULL;
802                        }
803
804                        // if there is only one child, then don't change direction - continue
805                        // along axis of parent. Otherwise calculate direction of id-th stick
806                        // (that is currstick) with use of RSZ/Vogel method of distributing points
807                        // evenly on a sphere
808                        if (count > 1)
809                        {
810                                direction = parent->firstpart->p - parent->secondpart->p;
811                                direction.normalize();
812                                // there has to be count+1 directions, so method needs to generate
813                                // count+1 evenly distributed points on a sphere to make vectors
814                                // from point (0,0,0) to those points. First generated vector
815                                // will act as parent joint direction vector
816                                Pt3D sphere0direction = getNextDirection(count + 1, 0);
817
818                                // First generated vector needs to be aligned to parent vector
819                                Orient rotmatrix = getRotationMatrixToFitVector(sphere0direction, direction);
820
821                                // Calculation of direction from sphere for currstick
822                                direction = getNextDirection(count + 1, id + 1);
823                                // Rotation matrix aligning
824                                direction = rotmatrix.transform(direction);
825                                direction.normalize();
826                        }
827                }
828
829                // calculate second position
830                par.select(currstick->obj);
831                secondposition += direction * par.getDoubleById("l");
832
833                // find every stick connected to current stick in order to calculate second
834                // part properties
835                vector<fH_StickHandle *> children;
836                currstick->secondparthandle = currstick->second;
837                for (pair<fH_StickHandle *, fH_StickHandle *> conn : stickconnections)
838                {
839                        if (conn.first == currstick)
840                        {
841                                children.push_back(conn.second);
842                                for (int i = 0; i < dimensions; i++)
843                                {
844                                        currstick->secondparthandle[i] += conn.second->first[i];
845                                }
846                        }
847                }
848                // create part from current stick and other sticks connected to this part
849                Part *secondpart = currstick->createPart(stickparamtab, &children, model, createmapping);
850                secondpart->p = secondposition;
851                currstick->secondpart = secondpart;
852                double count = (double)children.size() + 1;
853                for (int i = 0; i < dimensions; i++)
854                {
855                        currstick->secondparthandle[i] /= count;
856                }
857
858                //after creating second part connect two parts with joint
859                Joint * joint = currstick->createJoint(stickparamtab, model, createmapping);
860                if (!joint)
861                {
862                        logMessage("fH_Builder", "buildModel", LOG_ERROR, "Joint cannot be created");
863                        delete model;
864                        return NULL;
865
866                }
867                currstick->joint = joint;
868                model->checkpoint();
869        }
870        // after creating a body, attach neurons to body and link them according to
871        // connections
872        if (developBrain(model, createmapping) == -1)
873        {
874                delete model;
875                return NULL;
876        }
877        model->close();
878        return model;
879}
880
881int fH_Builder::removeNeuronsWithInvalidClasses()
882{
883        int count = neurons.size();
884        if (count == 0)
885        {
886                return 0;
887        }
888        vector<fH_NeuronHandle *>::iterator it = neurons.begin();
889        Param par(neuronparamtab, NULL);
890        while (it != neurons.end())
891        {
892                par.select((*it)->obj);
893                SString det = par.getStringById("d");
894                if (det == "")
895                {
896                        it++;
897                }
898                else
899                {
900                        Neuro *neu = new Neuro();
901                        neu->setDetails(det);
902                        if (neu->getClass())
903                        {
904                                it++;
905                        }
906                        else
907                        {
908                                fH_NeuronHandle *tmp = (*it);
909                                it = neurons.erase(it);
910                                delete tmp;
911                        }
912                        delete neu;
913                }
914
915        }
916        return count - neurons.size();
917}
918
919SString fH_Builder::toString()
920{
921        SString result = "";
922        result += to_string(dimensions).c_str();
923        result += "\n";
924        // first method stringifies parts
925        Param par(stickparamtab, NULL);
926        void *def = ParamObject::makeObject(stickparamtab);
927        par.select(def);
928        par.setDefault();
929        for (fH_StickHandle *currstick : sticks)
930        {
931                currstick->saveProperties(par);
932                SString props;
933                par.saveSingleLine(props, def, true, false);
934                result += "j:";
935                result += props;
936        }
937        ParamObject::freeObject(def);
938        par.setParamTab(neuronparamtab);
939        def = ParamObject::makeObject(neuronparamtab);
940        par.select(def);
941        par.setDefault();
942        for (fH_NeuronHandle *currneuron : neurons)
943        {
944                currneuron->saveProperties(par);
945                SString props;
946                par.saveSingleLine(props, def, true, false);
947                result += "n:";
948                result += props;
949        }
950        ParamObject::freeObject(def);
951        par.setParamTab(connectionparamtab);
952        def = ParamObject::makeObject(connectionparamtab);
953        par.select(def);
954        par.setDefault();
955        for (fH_ConnectionHandle *currconnection : connections)
956        {
957                currconnection->saveProperties(par);
958                SString props;
959                par.saveSingleLine(props, def, true, false);
960                result += "c:";
961                result += props;
962        }
963        ParamObject::freeObject(def);
964        return result;
965}
966
967ParamEntry* fH_Builder::getParamTab(fHBodyType type)
968{
969        switch (type)
970        {
971        case fHBodyType::JOINT:
972                return stickparamtab;
973                break;
974        case fHBodyType::NEURON:
975                return neuronparamtab;
976                break;
977        default:
978                return connectionparamtab;
979                break;
980        }
981}
Note: See TracBrowser for help on using the repository browser.