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

Last change on this file since 1181 was 973, checked in by Maciej Komosinski, 4 years ago

Increased SString and std::string compatibility: introduced length(), size(), and capacity(), and removed legacy methods that have std::string equivalents

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