1 | // This file is a part of Framsticks SDK. http://www.framsticks.com/ |
---|
2 | // Copyright (C) 2019-2020 Maciej Komosinski and Szymon Ulatowski. |
---|
3 | // See LICENSE.txt for details. |
---|
4 | |
---|
5 | #include <float.h> |
---|
6 | #include "fS_general.h" |
---|
7 | #include "frams/model/geometry/geometryutils.h" |
---|
8 | #include "frams/genetics/genooperators.h" |
---|
9 | #include "common/Convert.h" |
---|
10 | #include "frams/util/rndutil.h" |
---|
11 | #include "frams/neuro/neurolibrary.h" |
---|
12 | #include "../genooperators.h" |
---|
13 | |
---|
14 | int fS_Genotype::precision = 4; |
---|
15 | bool fS_Genotype::TURN_WITH_ROTATION = false; |
---|
16 | |
---|
17 | |
---|
18 | double fS_stod(const string& str, int start, size_t* size) |
---|
19 | { |
---|
20 | try |
---|
21 | { |
---|
22 | return std::stod(str, size); |
---|
23 | } |
---|
24 | catch(const std::invalid_argument& ex) |
---|
25 | { |
---|
26 | throw fS_Exception("Invalid numeric value", start); |
---|
27 | } |
---|
28 | catch(const std::out_of_range& ex) |
---|
29 | { |
---|
30 | throw fS_Exception("Invalid numeric value; out of range", start); |
---|
31 | } |
---|
32 | } |
---|
33 | |
---|
34 | State::State(State *_state) |
---|
35 | { |
---|
36 | location = Pt3D(_state->location); |
---|
37 | v = Pt3D(_state->v); |
---|
38 | fr = _state->fr; |
---|
39 | s = _state->s; |
---|
40 | stif = _state->stif; |
---|
41 | } |
---|
42 | |
---|
43 | State::State(Pt3D _location, Pt3D _v) |
---|
44 | { |
---|
45 | location = Pt3D(_location); |
---|
46 | v = Pt3D(_v); |
---|
47 | } |
---|
48 | |
---|
49 | void State::addVector(const double length) |
---|
50 | { |
---|
51 | location += v * length; |
---|
52 | } |
---|
53 | |
---|
54 | void rotateVector(Pt3D &vector, const Pt3D &rotation) |
---|
55 | { |
---|
56 | Orient rotmatrix = Orient_1; |
---|
57 | rotmatrix.rotate(rotation); |
---|
58 | vector = rotmatrix.transform(vector); |
---|
59 | } |
---|
60 | |
---|
61 | void State::rotate(const Pt3D &rotation) |
---|
62 | { |
---|
63 | rotateVector(v, rotation); |
---|
64 | v.normalize(); |
---|
65 | } |
---|
66 | |
---|
67 | |
---|
68 | fS_Neuron::fS_Neuron(const char *str, int start, int length) |
---|
69 | { |
---|
70 | if (length == 0) |
---|
71 | return; |
---|
72 | |
---|
73 | vector<SString> inputStrings; |
---|
74 | strSplit(SString(str, length), NEURON_INTERNAL_SEPARATOR, false, inputStrings); |
---|
75 | if (inputStrings.empty()) |
---|
76 | return; |
---|
77 | |
---|
78 | int inputStart = 0; |
---|
79 | SString details = "N"; |
---|
80 | |
---|
81 | SString tmp = inputStrings[0]; |
---|
82 | if(tmp.indexOf(':') != -1) |
---|
83 | tmp = tmp.substr(0, tmp.indexOf(':')); |
---|
84 | |
---|
85 | if (NeuroLibrary::staticlibrary.findClassIndex(tmp, true) != -1) |
---|
86 | { |
---|
87 | inputStart = 1; |
---|
88 | details = inputStrings[0]; |
---|
89 | } |
---|
90 | setDetails(details); |
---|
91 | |
---|
92 | for (int i = inputStart; i < int(inputStrings.size()); i++) |
---|
93 | { |
---|
94 | SString keyValue = inputStrings[i]; |
---|
95 | int separatorIndex = keyValue.indexOf(NEURON_I_W_SEPARATOR); |
---|
96 | const char *buffer = keyValue.c_str(); |
---|
97 | size_t keyLength; |
---|
98 | double value; |
---|
99 | if (separatorIndex == -1) |
---|
100 | { |
---|
101 | keyLength = keyValue.len(); |
---|
102 | value = DEFAULT_NEURO_CONNECTION_WEIGHT; |
---|
103 | } else |
---|
104 | { |
---|
105 | keyLength = separatorIndex; |
---|
106 | size_t valueLength = keyValue.len() - (separatorIndex); |
---|
107 | value = fS_stod(buffer + separatorIndex + 1, start, &valueLength); |
---|
108 | } |
---|
109 | inputs[fS_stod(buffer, start, &keyLength)] = value; |
---|
110 | } |
---|
111 | } |
---|
112 | |
---|
113 | Node::Node(Substring &restOfGeno, Node *_parent) |
---|
114 | { |
---|
115 | parent = _parent; |
---|
116 | partDescription = new Substring(restOfGeno); |
---|
117 | |
---|
118 | try |
---|
119 | { |
---|
120 | extractModifiers(restOfGeno); |
---|
121 | extractPartType(restOfGeno); |
---|
122 | extractNeurons(restOfGeno); |
---|
123 | extractParams(restOfGeno); |
---|
124 | |
---|
125 | partDescription->shortenBy(restOfGeno.len); |
---|
126 | if (restOfGeno.len > 0) |
---|
127 | getChildren(restOfGeno); |
---|
128 | } |
---|
129 | catch(fS_Exception &e) |
---|
130 | { |
---|
131 | cleanUp(); |
---|
132 | throw e; |
---|
133 | } |
---|
134 | } |
---|
135 | |
---|
136 | Node::~Node() |
---|
137 | { |
---|
138 | cleanUp(); |
---|
139 | } |
---|
140 | |
---|
141 | void Node::cleanUp() |
---|
142 | { |
---|
143 | delete partDescription; |
---|
144 | if (state != nullptr) |
---|
145 | delete state; |
---|
146 | for (int i = 0; i < int(neurons.size()); i++) |
---|
147 | delete neurons[i]; |
---|
148 | for (int i = 0; i < int(children.size()); i++) |
---|
149 | delete children[i]; |
---|
150 | } |
---|
151 | |
---|
152 | int Node::getPartPosition(Substring &restOfGenotype) |
---|
153 | { |
---|
154 | for (int i = 0; i < restOfGenotype.len; i++) |
---|
155 | { |
---|
156 | if (GENE_TO_SHAPETYPE.find(restOfGenotype.at(i)) != GENE_TO_SHAPETYPE.end()) |
---|
157 | return i; |
---|
158 | } |
---|
159 | return -1; |
---|
160 | } |
---|
161 | |
---|
162 | void Node::extractModifiers(Substring &restOfGenotype) |
---|
163 | { |
---|
164 | int partTypePosition = getPartPosition(restOfGenotype); |
---|
165 | if (partTypePosition == -1) |
---|
166 | throw fS_Exception("Part type missing", restOfGenotype.start); |
---|
167 | |
---|
168 | for (int i = 0; i < partTypePosition; i++) |
---|
169 | { |
---|
170 | // Extract modifiers and joint |
---|
171 | char mType = restOfGenotype.at(i); |
---|
172 | if (JOINTS.find(tolower(mType)) != string::npos) |
---|
173 | joint = tolower(mType); |
---|
174 | else if (MODIFIERS.find(toupper(mType)) != string::npos) |
---|
175 | modifiers[toupper(mType)] += isupper(mType) ? 1 : -1; |
---|
176 | else |
---|
177 | throw fS_Exception("Invalid modifier", restOfGenotype.start + i); |
---|
178 | } |
---|
179 | restOfGenotype.startFrom(partTypePosition); |
---|
180 | } |
---|
181 | |
---|
182 | void Node::extractPartType(Substring &restOfGenotype) |
---|
183 | { |
---|
184 | auto itr = GENE_TO_SHAPETYPE.find(restOfGenotype.at(0)); |
---|
185 | if (itr == GENE_TO_SHAPETYPE.end()) |
---|
186 | throw fS_Exception("Invalid part type", restOfGenotype.start); |
---|
187 | |
---|
188 | partType = itr->second; |
---|
189 | restOfGenotype.startFrom(1); |
---|
190 | } |
---|
191 | |
---|
192 | vector<int> getSeparatorPositions(const char *str, int len, char separator, char endSign, int &endIndex) |
---|
193 | { |
---|
194 | endIndex = -1; |
---|
195 | vector<int> separators {-1}; |
---|
196 | for (int i = 0; i < len; i++) |
---|
197 | { |
---|
198 | if (str[i] == separator) |
---|
199 | separators.push_back(i); |
---|
200 | else if (str[i] == endSign) |
---|
201 | { |
---|
202 | endIndex = i; |
---|
203 | break; |
---|
204 | } |
---|
205 | } |
---|
206 | separators.push_back(endIndex); // End of string as last separator |
---|
207 | return separators; |
---|
208 | } |
---|
209 | |
---|
210 | void Node::extractNeurons(Substring &restOfGenotype) |
---|
211 | { |
---|
212 | if (restOfGenotype.len == 0 || restOfGenotype.at(0) != NEURON_START) |
---|
213 | return; |
---|
214 | |
---|
215 | const char *ns = restOfGenotype.c_str() + 1; |
---|
216 | int neuronsEndIndex; |
---|
217 | vector<int> separators = getSeparatorPositions(ns, restOfGenotype.len, NEURON_SEPARATOR, NEURON_END, neuronsEndIndex); |
---|
218 | if(neuronsEndIndex == -1) |
---|
219 | throw fS_Exception("Lacking neuro end sign", restOfGenotype.start); |
---|
220 | |
---|
221 | for (int i = 0; i < int(separators.size()) - 1; i++) |
---|
222 | { |
---|
223 | int start = separators[i] + 1; |
---|
224 | int length = separators[i + 1] - start; |
---|
225 | fS_Neuron *newNeuron = new fS_Neuron(ns + start, restOfGenotype.start + start, length); |
---|
226 | neurons.push_back(newNeuron); |
---|
227 | } |
---|
228 | |
---|
229 | restOfGenotype.startFrom(neuronsEndIndex + 2); |
---|
230 | } |
---|
231 | |
---|
232 | void Node::extractParams(Substring &restOfGenotype) |
---|
233 | { |
---|
234 | if (restOfGenotype.len == 0 || restOfGenotype.at(0) != PARAM_START) |
---|
235 | return; |
---|
236 | |
---|
237 | const char *paramString = restOfGenotype.c_str() + 1; |
---|
238 | |
---|
239 | // Find the indexes of the parameter separators |
---|
240 | int paramsEndIndex; |
---|
241 | vector<int> separators = getSeparatorPositions(paramString, restOfGenotype.len, PARAM_SEPARATOR, PARAM_END, paramsEndIndex); |
---|
242 | if(paramsEndIndex == -1) |
---|
243 | throw fS_Exception("Lacking param end sign", restOfGenotype.start); |
---|
244 | for (int i = 0; i < int(separators.size()) - 1; i++) |
---|
245 | { |
---|
246 | int start = separators[i] + 1; |
---|
247 | int length = separators[i + 1] - start; |
---|
248 | const char *buffer = paramString + start; |
---|
249 | |
---|
250 | // Find the index of key-value separator |
---|
251 | int separatorIndex = -1; |
---|
252 | for (int i = 0; i < length; i++) |
---|
253 | { |
---|
254 | if (buffer[i] == PARAM_KEY_VALUE_SEPARATOR) |
---|
255 | { |
---|
256 | separatorIndex = i; |
---|
257 | break; |
---|
258 | } |
---|
259 | } |
---|
260 | if (-1 == separatorIndex) |
---|
261 | throw fS_Exception("Parameter separator expected", restOfGenotype.start); |
---|
262 | |
---|
263 | // Compute the value of parameter and assign it to the key |
---|
264 | int valueStartIndex = separatorIndex + 1; |
---|
265 | string key(buffer, separatorIndex); |
---|
266 | if(std::find(PARAMS.begin(), PARAMS.end(), key) == PARAMS.end()) |
---|
267 | throw fS_Exception("Invalid parameter key", restOfGenotype.start + start); |
---|
268 | |
---|
269 | const char *val = buffer + valueStartIndex; |
---|
270 | size_t len = length - valueStartIndex; |
---|
271 | double value = fS_stod(val, restOfGenotype.start + start + valueStartIndex, &len); |
---|
272 | if((key==SIZE_X || key==SIZE_Y || key==SIZE_Z) && value <= 0.0) |
---|
273 | throw fS_Exception("Invalid value of radius parameter", restOfGenotype.start + start + valueStartIndex); |
---|
274 | |
---|
275 | params[key] = value; |
---|
276 | |
---|
277 | } |
---|
278 | |
---|
279 | restOfGenotype.startFrom(paramsEndIndex + 2); |
---|
280 | } |
---|
281 | |
---|
282 | double Node::getParam(string key) |
---|
283 | { |
---|
284 | auto item = params.find(key); |
---|
285 | if (item != params.end()) |
---|
286 | return item->second; |
---|
287 | else |
---|
288 | return defaultValues.at(key); |
---|
289 | } |
---|
290 | |
---|
291 | double avg(double a, double b) |
---|
292 | { |
---|
293 | return 0.5 * (a + b); |
---|
294 | } |
---|
295 | |
---|
296 | double min3(Pt3D p) |
---|
297 | { |
---|
298 | double tmp = p.x; |
---|
299 | if (p.y < tmp) |
---|
300 | tmp = p.y; |
---|
301 | if (p.z < tmp) |
---|
302 | tmp = p.z; |
---|
303 | return tmp; |
---|
304 | } |
---|
305 | |
---|
306 | double max3(Pt3D p) |
---|
307 | { |
---|
308 | double tmp = p.x; |
---|
309 | if (p.y > tmp) |
---|
310 | tmp = p.y; |
---|
311 | if (p.z > tmp) |
---|
312 | tmp = p.z; |
---|
313 | return tmp; |
---|
314 | } |
---|
315 | |
---|
316 | double getSphereCoordinate(double dimension, double sphereDiameter, double index, int count) |
---|
317 | { |
---|
318 | if (count == 1) |
---|
319 | return 0; |
---|
320 | return (dimension - sphereDiameter) * (index / (count - 1) - 0.5); |
---|
321 | } |
---|
322 | |
---|
323 | Pt3D *findSphereCenters(int &sphereCount, double &sphereRadius, Pt3D radii, Pt3D rotations) |
---|
324 | { |
---|
325 | double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE; |
---|
326 | double minRadius = min3(radii); |
---|
327 | if(minRadius <= 0) |
---|
328 | throw fS_Exception("Invalid part size", 0); |
---|
329 | double maxRadius = max3(radii); |
---|
330 | if (MAX_DIAMETER_QUOTIENT > maxRadius / minRadius) |
---|
331 | sphereRadius = minRadius; |
---|
332 | else |
---|
333 | { |
---|
334 | // When max radius is much bigger than min radius |
---|
335 | sphereRelativeDistance = 1.0; // Make the spheres adjacent to speed up the computation |
---|
336 | sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT; |
---|
337 | } |
---|
338 | double sphereDiameter = 2 * sphereRadius; |
---|
339 | |
---|
340 | double *diameters = new double[3] {2 * radii.x, 2 * radii.y, 2 * radii.z}; |
---|
341 | int counts[3]; |
---|
342 | for (int i = 0; i < 3; i++) |
---|
343 | { |
---|
344 | counts[i] = 1; |
---|
345 | if (diameters[i] > sphereDiameter) |
---|
346 | counts[i] += ceil((diameters[i] - sphereDiameter) / sphereDiameter / sphereRelativeDistance); |
---|
347 | } |
---|
348 | |
---|
349 | sphereCount = counts[0] * counts[1] * counts[2]; |
---|
350 | double x, y, z; |
---|
351 | int totalCount = 0; |
---|
352 | Pt3D *centers = new Pt3D[sphereCount]; |
---|
353 | for (double xi = 0; xi < counts[0]; xi++) |
---|
354 | { |
---|
355 | x = getSphereCoordinate(diameters[0], sphereDiameter, xi, counts[0]); |
---|
356 | for (double yi = 0; yi < counts[1]; yi++) |
---|
357 | { |
---|
358 | y = getSphereCoordinate(diameters[1], sphereDiameter, yi, counts[1]); |
---|
359 | for (double zi = 0; zi < counts[2]; zi++) |
---|
360 | { |
---|
361 | z = getSphereCoordinate(diameters[2], sphereDiameter, zi, counts[2]); |
---|
362 | centers[totalCount] = Pt3D(x, y, z); |
---|
363 | rotateVector(centers[totalCount], rotations); |
---|
364 | totalCount++; |
---|
365 | } |
---|
366 | } |
---|
367 | } |
---|
368 | delete[] diameters; |
---|
369 | return centers; |
---|
370 | } |
---|
371 | |
---|
372 | int isCollision(Pt3D *centersParent, Pt3D *centers, int parentSphereCount, int sphereCount, Pt3D &vector, |
---|
373 | double distanceThreshold) |
---|
374 | { |
---|
375 | double upperThreshold = distanceThreshold; |
---|
376 | double lowerThreshold = SPHERE_DISTANCE_TOLERANCE * distanceThreshold; |
---|
377 | double distance; |
---|
378 | double dx, dy, dz; |
---|
379 | bool existsAdjacent = false; |
---|
380 | Pt3D *tmpPoint; |
---|
381 | for (int sc = 0; sc < sphereCount; sc++) |
---|
382 | { |
---|
383 | Pt3D shiftedSphere = Pt3D(centers[sc]); |
---|
384 | shiftedSphere += vector; |
---|
385 | for (int psc = 0; psc < parentSphereCount; psc++) |
---|
386 | { |
---|
387 | tmpPoint = ¢ersParent[psc]; |
---|
388 | dx = shiftedSphere.x - tmpPoint->x; |
---|
389 | dy = shiftedSphere.y - tmpPoint->y; |
---|
390 | dz = shiftedSphere.z - tmpPoint->z; |
---|
391 | distance = sqrt(dx * dx + dy * dy + dz * dz); |
---|
392 | |
---|
393 | if (distance <= upperThreshold) |
---|
394 | { |
---|
395 | if (distance >= lowerThreshold) |
---|
396 | existsAdjacent = true; |
---|
397 | else |
---|
398 | { |
---|
399 | return COLLISION; |
---|
400 | } |
---|
401 | } |
---|
402 | } |
---|
403 | } |
---|
404 | if (existsAdjacent) |
---|
405 | return ADJACENT; |
---|
406 | else |
---|
407 | return DISJOINT; |
---|
408 | } |
---|
409 | |
---|
410 | double Node::getDistance() |
---|
411 | { |
---|
412 | Pt3D size = calculateSize(); |
---|
413 | Pt3D parentSize = parent->calculateSize(); // Here we are sure that parent is not nullptr |
---|
414 | int parentSphereCount, sphereCount; |
---|
415 | double parentSphereRadius, sphereRadius; |
---|
416 | Pt3D *centersParent = findSphereCenters(parentSphereCount, parentSphereRadius, parentSize, parent->getRotation()); |
---|
417 | Pt3D *centers = findSphereCenters(sphereCount, sphereRadius, size, getRotation()); |
---|
418 | |
---|
419 | double distanceThreshold = sphereRadius + parentSphereRadius; |
---|
420 | double minDistance = 0.0; |
---|
421 | double maxDistance = 2 * (max3(parentSize) + max3(size)); |
---|
422 | double currentDistance = avg(maxDistance, minDistance); |
---|
423 | int result = -1; |
---|
424 | int iterationNo = 0; |
---|
425 | while (result != ADJACENT) |
---|
426 | { |
---|
427 | iterationNo++; |
---|
428 | Pt3D currentVector = state->v * currentDistance; |
---|
429 | result = isCollision(centersParent, centers, parentSphereCount, sphereCount, currentVector, distanceThreshold); |
---|
430 | |
---|
431 | if (result == DISJOINT) |
---|
432 | { |
---|
433 | maxDistance = currentDistance; |
---|
434 | currentDistance = avg(currentDistance, minDistance); |
---|
435 | } else if (result == COLLISION) |
---|
436 | { |
---|
437 | minDistance = currentDistance; |
---|
438 | currentDistance = avg(maxDistance, currentDistance); |
---|
439 | } |
---|
440 | |
---|
441 | if(maxDistance <= 0 || iterationNo > 1000) |
---|
442 | throw fS_Exception("Computing of distances between parts failed", 0); |
---|
443 | if (currentDistance > maxDistance) |
---|
444 | { |
---|
445 | throw fS_Exception("Internal error; then maximal distance between parts exceeded.", 0); |
---|
446 | } |
---|
447 | if (currentDistance < minDistance) |
---|
448 | throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0); |
---|
449 | |
---|
450 | } |
---|
451 | |
---|
452 | delete[] centersParent; |
---|
453 | delete[] centers; |
---|
454 | return currentDistance; |
---|
455 | } |
---|
456 | |
---|
457 | void Node::getState(State *_state) |
---|
458 | { |
---|
459 | if (state != nullptr) |
---|
460 | delete state; |
---|
461 | if (parent == nullptr) |
---|
462 | state = _state; |
---|
463 | else |
---|
464 | state = new State(_state); |
---|
465 | |
---|
466 | |
---|
467 | // Update state by modifiers |
---|
468 | for (auto it = modifiers.begin(); it != modifiers.end(); ++it) |
---|
469 | { |
---|
470 | char mod = it->first; |
---|
471 | double multiplier = pow(MODIFIER_MULTIPLIER, it->second); |
---|
472 | if (mod == MODIFIERS[0]) |
---|
473 | state->ing *= multiplier; |
---|
474 | else if (mod == MODIFIERS[1]) |
---|
475 | state->fr *= multiplier; |
---|
476 | else if (mod == MODIFIERS[2]) |
---|
477 | state->s *= multiplier; |
---|
478 | else if (mod == MODIFIERS[3]) |
---|
479 | state->stif *= multiplier; |
---|
480 | } |
---|
481 | |
---|
482 | if (parent != nullptr) |
---|
483 | { |
---|
484 | // Rotate |
---|
485 | state->rotate(getVectorRotation()); |
---|
486 | |
---|
487 | double distance = getDistance(); |
---|
488 | state->addVector(distance); |
---|
489 | } |
---|
490 | for (int i = 0; i < int(children.size()); i++) |
---|
491 | children[i]->getState(state); |
---|
492 | } |
---|
493 | |
---|
494 | void Node::getChildren(Substring &restOfGenotype) |
---|
495 | { |
---|
496 | vector<Substring> branches = getBranches(restOfGenotype); |
---|
497 | for (int i = 0; i < int(branches.size()); i++) |
---|
498 | { |
---|
499 | children.push_back(new Node(branches[i], this)); |
---|
500 | } |
---|
501 | } |
---|
502 | |
---|
503 | vector<Substring> Node::getBranches(Substring &restOfGenotype) |
---|
504 | { |
---|
505 | vector<Substring> children; |
---|
506 | if (restOfGenotype.at(0) != BRANCH_START) |
---|
507 | { |
---|
508 | children.push_back(restOfGenotype); // Only one child |
---|
509 | return children; |
---|
510 | } |
---|
511 | |
---|
512 | int depth = 0; |
---|
513 | int start = 1; |
---|
514 | char c; |
---|
515 | const char *str = restOfGenotype.c_str(); |
---|
516 | for (int i = 0; i < restOfGenotype.len; i++) |
---|
517 | { |
---|
518 | if (depth < 0) |
---|
519 | throw fS_Exception("The number of branch start signs does not equal the number of branch end signs", restOfGenotype.start + i); |
---|
520 | c = str[i]; |
---|
521 | if (c == BRANCH_START) |
---|
522 | depth++; |
---|
523 | else if ((c == BRANCH_SEPARATOR && depth == 1) || i + 1 == restOfGenotype.len) |
---|
524 | { |
---|
525 | Substring substring(restOfGenotype); |
---|
526 | substring.startFrom(start); |
---|
527 | substring.len = i - start; |
---|
528 | children.push_back(substring); |
---|
529 | start = i + 1; |
---|
530 | } else if (c == BRANCH_END) |
---|
531 | depth--; |
---|
532 | } |
---|
533 | if (depth != 1) // T |
---|
534 | throw fS_Exception("The number of branch start signs does not equal the number of branch end signs", restOfGenotype.start); |
---|
535 | return children; |
---|
536 | } |
---|
537 | |
---|
538 | Pt3D Node::calculateSize() |
---|
539 | { |
---|
540 | double sizeMultiplier = getParam(SIZE) * state->s; |
---|
541 | double sx = getParam(SIZE_X) * sizeMultiplier; |
---|
542 | double sy = getParam(SIZE_Y) * sizeMultiplier; |
---|
543 | double sz = getParam(SIZE_Z) * sizeMultiplier; |
---|
544 | return Pt3D(sx, sy, sz); |
---|
545 | } |
---|
546 | |
---|
547 | double Node::calculateVolume() |
---|
548 | { |
---|
549 | double result; |
---|
550 | Pt3D size = calculateSize(); |
---|
551 | double radiiProduct = size.x * size.y * size.z; |
---|
552 | switch (partType) |
---|
553 | { |
---|
554 | case Part::Shape::SHAPE_CUBOID: |
---|
555 | result = 8.0 * radiiProduct; |
---|
556 | break; |
---|
557 | case Part::Shape::SHAPE_CYLINDER: |
---|
558 | result = 2.0 * M_PI * radiiProduct; |
---|
559 | break; |
---|
560 | case Part::Shape::SHAPE_ELLIPSOID: |
---|
561 | result = (4.0 / 3.0) * M_PI * radiiProduct; |
---|
562 | break; |
---|
563 | default: |
---|
564 | logMessage("fS", "calculateVolume", LOG_ERROR, "Invalid part type"); |
---|
565 | } |
---|
566 | return result; |
---|
567 | } |
---|
568 | |
---|
569 | bool Node::isPartSizeValid() |
---|
570 | { |
---|
571 | Pt3D size = calculateSize(); |
---|
572 | double volume = calculateVolume(); |
---|
573 | Part_MinMaxDef minP = Model::getMinPart(); |
---|
574 | Part_MinMaxDef maxP = Model::getMaxPart(); |
---|
575 | |
---|
576 | if (volume > maxP.volume || minP.volume > volume) |
---|
577 | return false; |
---|
578 | if (size.x < minP.scale.x || size.y < minP.scale.y || size.z < minP.scale.z) |
---|
579 | return false; |
---|
580 | if (size.x > maxP.scale.x || size.y > maxP.scale.y || size.z > maxP.scale.z) |
---|
581 | return false; |
---|
582 | |
---|
583 | if (partType == Part::Shape::SHAPE_ELLIPSOID && max3(size) != min3(size)) |
---|
584 | // When not all radii have different values |
---|
585 | return false; |
---|
586 | if (partType == Part::Shape::SHAPE_CYLINDER && size.x != size.y) |
---|
587 | // If base radii have different values |
---|
588 | return false; |
---|
589 | return true; |
---|
590 | } |
---|
591 | |
---|
592 | bool Node::hasPartSizeParam() |
---|
593 | { |
---|
594 | return params.count(SIZE_X) > 0 || params.count(SIZE_Y) > 0 || params.count(SIZE_Z) > 0; |
---|
595 | } |
---|
596 | |
---|
597 | Pt3D Node::getVectorRotation() |
---|
598 | { |
---|
599 | return Pt3D(getParam(ROT_X), getParam(ROT_Y), getParam(ROT_Z)); |
---|
600 | } |
---|
601 | |
---|
602 | Pt3D Node::getRotation() |
---|
603 | { |
---|
604 | Pt3D rotation = Pt3D(getParam(RX), getParam(RY), getParam(RZ)); |
---|
605 | if(fS_Genotype::TURN_WITH_ROTATION) |
---|
606 | rotation += getVectorRotation(); |
---|
607 | return rotation; |
---|
608 | } |
---|
609 | |
---|
610 | void Node::buildModel(Model &model, Node *parent) |
---|
611 | { |
---|
612 | createPart(); |
---|
613 | model.addPart(part); |
---|
614 | if (parent != nullptr) |
---|
615 | addJointsToModel(model, parent); |
---|
616 | |
---|
617 | for (int i = 0; i < int(neurons.size()); i++) |
---|
618 | { |
---|
619 | Neuro *neuro = new Neuro(*neurons[i]); |
---|
620 | model.addNeuro(neuro); |
---|
621 | if (neuro->getClass()->preflocation == 2 && parent != nullptr) |
---|
622 | { |
---|
623 | neuro->attachToJoint(model.getJoint(model.getJointCount() - 1)); |
---|
624 | } else |
---|
625 | neuro->attachToPart(part); |
---|
626 | } |
---|
627 | |
---|
628 | model.checkpoint(); |
---|
629 | part->addMapping(partDescription->toMultiRange()); |
---|
630 | |
---|
631 | for (int i = 0; i < int(children.size()); i++) |
---|
632 | { |
---|
633 | Node *child = children[i]; |
---|
634 | child->buildModel(model, this); |
---|
635 | } |
---|
636 | } |
---|
637 | |
---|
638 | void Node::createPart() |
---|
639 | { |
---|
640 | part = new Part(partType); |
---|
641 | part->p = Pt3D(state->location.x, |
---|
642 | state->location.y, |
---|
643 | state->location.z); |
---|
644 | |
---|
645 | part->friction = getParam(FRICTION) * state->fr; |
---|
646 | part->ingest = getParam(INGESTION) * state->ing; |
---|
647 | Pt3D size = calculateSize(); |
---|
648 | part->scale.x = size.x; |
---|
649 | part->scale.y = size.y; |
---|
650 | part->scale.z = size.z; |
---|
651 | part->setRot(getRotation()); |
---|
652 | } |
---|
653 | |
---|
654 | void Node::addJointsToModel(Model &model, Node *parent) |
---|
655 | { |
---|
656 | Joint *j = new Joint(); |
---|
657 | j->stif = getParam(STIFFNESS) * state->stif; |
---|
658 | j->rotstif = j->stif; |
---|
659 | |
---|
660 | j->attachToParts(parent->part, part); |
---|
661 | switch (joint) |
---|
662 | { |
---|
663 | case HINGE_X: |
---|
664 | j->shape = Joint::Shape::SHAPE_HINGE_X; |
---|
665 | break; |
---|
666 | case HINGE_XY: |
---|
667 | j->shape = Joint::Shape::SHAPE_HINGE_XY; |
---|
668 | break; |
---|
669 | default: |
---|
670 | j->shape = Joint::Shape::SHAPE_FIXED; |
---|
671 | } |
---|
672 | model.addJoint(j); |
---|
673 | j->addMapping(partDescription->toMultiRange()); |
---|
674 | } |
---|
675 | |
---|
676 | |
---|
677 | void Node::getGeno(SString &result) |
---|
678 | { |
---|
679 | if (joint != DEFAULT_JOINT) |
---|
680 | result += joint; |
---|
681 | for (auto it = modifiers.begin(); it != modifiers.end(); ++it) |
---|
682 | { |
---|
683 | char mod = it->first; |
---|
684 | int count = it->second; |
---|
685 | if(it->second < 0) |
---|
686 | { |
---|
687 | mod = tolower(mod); |
---|
688 | count = fabs(count); |
---|
689 | } |
---|
690 | result += std::string(count, mod).c_str(); |
---|
691 | } |
---|
692 | result += SHAPETYPE_TO_GENE.at(partType); |
---|
693 | |
---|
694 | if (!neurons.empty()) |
---|
695 | { |
---|
696 | // Add neurons to genotype string |
---|
697 | result += NEURON_START; |
---|
698 | for (int i = 0; i < int(neurons.size()); i++) |
---|
699 | { |
---|
700 | fS_Neuron *n = neurons[i]; |
---|
701 | if (i != 0) |
---|
702 | result += NEURON_SEPARATOR; |
---|
703 | |
---|
704 | result += n->getDetails(); |
---|
705 | if (!n->inputs.empty()) |
---|
706 | result += NEURON_INTERNAL_SEPARATOR; |
---|
707 | |
---|
708 | for (auto it = n->inputs.begin(); it != n->inputs.end(); ++it) |
---|
709 | { |
---|
710 | if (it != n->inputs.begin()) |
---|
711 | result += NEURON_INTERNAL_SEPARATOR; |
---|
712 | result += SString::valueOf(it->first); |
---|
713 | if (it->second != DEFAULT_NEURO_CONNECTION_WEIGHT) |
---|
714 | { |
---|
715 | result += NEURON_I_W_SEPARATOR; |
---|
716 | result += SString::valueOf(it->second); |
---|
717 | } |
---|
718 | } |
---|
719 | } |
---|
720 | result += NEURON_END; |
---|
721 | } |
---|
722 | |
---|
723 | if (!params.empty()) |
---|
724 | { |
---|
725 | // Add parameters to genotype string |
---|
726 | result += PARAM_START; |
---|
727 | for (auto it = params.begin(); it != params.end(); ++it) |
---|
728 | { |
---|
729 | if (it != params.begin()) |
---|
730 | result += PARAM_SEPARATOR; |
---|
731 | |
---|
732 | result += it->first.c_str(); // Add parameter key to string |
---|
733 | result += PARAM_KEY_VALUE_SEPARATOR; |
---|
734 | string value_text = std::to_string(it->second); |
---|
735 | // Round the value to two decimal places and add to string |
---|
736 | result += value_text.substr(0, value_text.find(".") + fS_Genotype::precision).c_str(); |
---|
737 | } |
---|
738 | result += PARAM_END; |
---|
739 | } |
---|
740 | |
---|
741 | if (children.size() == 1) |
---|
742 | children[0]->getGeno(result); |
---|
743 | else if (children.size() > 1) |
---|
744 | { |
---|
745 | result += BRANCH_START; |
---|
746 | for (int i = 0; i < int(children.size()) - 1; i++) |
---|
747 | { |
---|
748 | children[i]->getGeno(result); |
---|
749 | result += BRANCH_SEPARATOR; |
---|
750 | } |
---|
751 | children.back()->getGeno(result); |
---|
752 | result += BRANCH_END; |
---|
753 | } |
---|
754 | } |
---|
755 | |
---|
756 | |
---|
757 | bool Node::changeSizeParam(string key, bool ensureCircleSection) |
---|
758 | { |
---|
759 | double oldValue = getParam(key); |
---|
760 | params[key] = GenoOperators::mutateCreep('f', params[key], minValues.at(key), maxValues.at(key), true); |
---|
761 | if (!ensureCircleSection || isPartSizeValid()) |
---|
762 | return true; |
---|
763 | else |
---|
764 | { |
---|
765 | params[key] = oldValue; |
---|
766 | return false; |
---|
767 | } |
---|
768 | } |
---|
769 | |
---|
770 | void Node::getAllNodes(vector<Node *> &allNodes) |
---|
771 | { |
---|
772 | allNodes.push_back(this); |
---|
773 | for (int i = 0; i < int(children.size()); i++) |
---|
774 | children[i]->getAllNodes(allNodes); |
---|
775 | } |
---|
776 | |
---|
777 | int Node::getNodeCount() |
---|
778 | { |
---|
779 | vector<Node*> allNodes; |
---|
780 | getAllNodes(allNodes); |
---|
781 | return allNodes.size(); |
---|
782 | } |
---|
783 | |
---|
784 | fS_Genotype::fS_Genotype(const string &genotype) |
---|
785 | { |
---|
786 | try |
---|
787 | { |
---|
788 | string geno = genotype.c_str(); |
---|
789 | Substring substring(geno.c_str(), 0, geno.length()); |
---|
790 | startNode = new Node(substring, nullptr); |
---|
791 | validateNeuroInputs(); |
---|
792 | } |
---|
793 | catch (fS_Exception &e) |
---|
794 | { |
---|
795 | delete startNode; |
---|
796 | throw e; |
---|
797 | } |
---|
798 | } |
---|
799 | |
---|
800 | fS_Genotype::~fS_Genotype() |
---|
801 | { |
---|
802 | delete startNode; |
---|
803 | } |
---|
804 | |
---|
805 | void fS_Genotype::getState() |
---|
806 | { |
---|
807 | State *initialState = new State(Pt3D(0), Pt3D(1, 0, 0)); |
---|
808 | startNode->getState(initialState); |
---|
809 | } |
---|
810 | |
---|
811 | void fS_Genotype::buildModel(Model &model) |
---|
812 | { |
---|
813 | getState(); |
---|
814 | startNode->buildModel(model, nullptr); |
---|
815 | buildNeuroConnections(model); |
---|
816 | } |
---|
817 | |
---|
818 | |
---|
819 | void fS_Genotype::buildNeuroConnections(Model &model) |
---|
820 | { |
---|
821 | // All the neurons are already created in the model |
---|
822 | vector<fS_Neuron*> allNeurons = getAllNeurons(); |
---|
823 | for (int i = 0; i < int(allNeurons.size()); i++) |
---|
824 | { |
---|
825 | fS_Neuron *neuron = allNeurons[i]; |
---|
826 | Neuro *modelNeuro = model.getNeuro(i); |
---|
827 | for (auto it = neuron->inputs.begin(); it != neuron->inputs.end(); ++it) |
---|
828 | { |
---|
829 | Neuro *inputNeuro = model.getNeuro(it->first); |
---|
830 | modelNeuro->addInput(inputNeuro, it->second); |
---|
831 | |
---|
832 | } |
---|
833 | } |
---|
834 | } |
---|
835 | |
---|
836 | Node *fS_Genotype::getNearestNode(vector<Node *> allNodes, Node *node) |
---|
837 | { |
---|
838 | Node *result = nullptr; |
---|
839 | double minDistance = DBL_MAX, distance = DBL_MAX; |
---|
840 | for (int i = 0; i < int(allNodes.size()); i++) |
---|
841 | { |
---|
842 | Node *otherNode = allNodes[i]; |
---|
843 | auto v = node->children; |
---|
844 | if (otherNode != node && |
---|
845 | find(v.begin(), v.end(), otherNode) == v.end()) |
---|
846 | { // Not the same node and not a child |
---|
847 | distance = node->state->location.distanceTo(otherNode->state->location); |
---|
848 | if (distance < minDistance) |
---|
849 | { |
---|
850 | minDistance = distance; |
---|
851 | result = otherNode; |
---|
852 | } |
---|
853 | } |
---|
854 | } |
---|
855 | return result; |
---|
856 | } |
---|
857 | |
---|
858 | SString fS_Genotype::getGeno() |
---|
859 | { |
---|
860 | SString geno; |
---|
861 | geno.memoryHint(100); // Provide a small buffer from the start to improve performance |
---|
862 | startNode->getGeno(geno); |
---|
863 | return geno; |
---|
864 | } |
---|
865 | |
---|
866 | vector<fS_Neuron *> fS_Genotype::extractNeurons(Node *node) |
---|
867 | { |
---|
868 | vector<Node*> allNodes; |
---|
869 | node->getAllNodes(allNodes); |
---|
870 | |
---|
871 | vector<fS_Neuron*> allNeurons; |
---|
872 | for (int i = 0; i < int(allNodes.size()); i++) |
---|
873 | { |
---|
874 | for (int j = 0; j < int(allNodes[i]->neurons.size()); j++) |
---|
875 | { |
---|
876 | allNeurons.push_back(allNodes[i]->neurons[j]); |
---|
877 | } |
---|
878 | } |
---|
879 | return allNeurons; |
---|
880 | } |
---|
881 | |
---|
882 | int fS_Genotype::getNeuronIndex(vector<fS_Neuron *> neurons, fS_Neuron *changedNeuron) |
---|
883 | { |
---|
884 | int neuronIndex = -1; |
---|
885 | for (int i = 0; i < int(neurons.size()); i++) |
---|
886 | { |
---|
887 | if (changedNeuron == neurons[i]) |
---|
888 | { |
---|
889 | neuronIndex = i; |
---|
890 | break; |
---|
891 | } |
---|
892 | } |
---|
893 | return neuronIndex; |
---|
894 | } |
---|
895 | |
---|
896 | void fS_Genotype::shiftNeuroConnections(vector<fS_Neuron *> &neurons, int start, int end, SHIFT shift) |
---|
897 | { |
---|
898 | if (start == -1 || end == -1) |
---|
899 | return; |
---|
900 | int shiftValue = end - start + 1; |
---|
901 | if (shift == SHIFT::LEFT) |
---|
902 | shiftValue *= -1; |
---|
903 | |
---|
904 | for (int i = 0; i < int(neurons.size()); i++) |
---|
905 | { |
---|
906 | fS_Neuron *n = neurons[i]; |
---|
907 | std::map<int, double> newInputs; |
---|
908 | for (auto it = n->inputs.begin(); it != n->inputs.end(); ++it) |
---|
909 | { |
---|
910 | if (start > it->first) |
---|
911 | newInputs[it->first] = it->second; |
---|
912 | else if (it->first >= start) |
---|
913 | { |
---|
914 | if (end >= it->first) |
---|
915 | { |
---|
916 | if (shift == SHIFT::RIGHT) |
---|
917 | newInputs[it->first + shiftValue] = it->second; |
---|
918 | // If shift == -1, just delete the input |
---|
919 | } else if (it->first > end) |
---|
920 | newInputs[it->first + shiftValue] = it->second; |
---|
921 | } |
---|
922 | } |
---|
923 | n->inputs = newInputs; |
---|
924 | } |
---|
925 | } |
---|
926 | |
---|
927 | vector<Node *> fS_Genotype::getAllNodes() |
---|
928 | { |
---|
929 | vector<Node*> allNodes; |
---|
930 | startNode->getAllNodes(allNodes); |
---|
931 | return allNodes; |
---|
932 | } |
---|
933 | |
---|
934 | vector<fS_Neuron *> fS_Genotype::getAllNeurons() |
---|
935 | { |
---|
936 | return extractNeurons(startNode); |
---|
937 | } |
---|
938 | |
---|
939 | Node *fS_Genotype::chooseNode(int fromIndex) |
---|
940 | { |
---|
941 | vector<Node*> allNodes = getAllNodes(); |
---|
942 | return allNodes[fromIndex + rndUint(allNodes.size() - fromIndex)]; |
---|
943 | } |
---|
944 | |
---|
945 | int fS_Genotype::getNodeCount() |
---|
946 | { |
---|
947 | return startNode->getNodeCount(); |
---|
948 | } |
---|
949 | |
---|
950 | int fS_Genotype::checkValidityOfPartSizes() |
---|
951 | { |
---|
952 | getState(); |
---|
953 | vector<Node*> nodes = getAllNodes(); |
---|
954 | for (int i = 0; i < int(nodes.size()); i++) |
---|
955 | { |
---|
956 | if (!nodes[i]->isPartSizeValid()) |
---|
957 | { |
---|
958 | return 1 + nodes[i]->partDescription->start; |
---|
959 | } |
---|
960 | } |
---|
961 | return 0; |
---|
962 | } |
---|
963 | |
---|
964 | |
---|
965 | void fS_Genotype::validateNeuroInputs() |
---|
966 | { |
---|
967 | |
---|
968 | // Validate neuro input numbers |
---|
969 | vector<fS_Neuron*> allNeurons = getAllNeurons(); |
---|
970 | int allNeuronsSize = allNeurons.size(); |
---|
971 | for(int i=0; i<allNeuronsSize; i++) |
---|
972 | { |
---|
973 | fS_Neuron *n = allNeurons[i]; |
---|
974 | for (auto it = n->inputs.begin(); it != n->inputs.end(); ++it) |
---|
975 | { |
---|
976 | if (it->first < 0 || it->first >= allNeuronsSize) |
---|
977 | throw fS_Exception("Invalid neuron input", 0); |
---|
978 | } |
---|
979 | } |
---|
980 | } |
---|
981 | |
---|
982 | |
---|
983 | void fS_Genotype::rearrangeNeuronConnections(fS_Neuron *changedNeuron, SHIFT shift) |
---|
984 | { |
---|
985 | vector<fS_Neuron*> neurons = getAllNeurons(); |
---|
986 | int changedNeuronIndex = getNeuronIndex(neurons, changedNeuron); |
---|
987 | shiftNeuroConnections(neurons, changedNeuronIndex, changedNeuronIndex, shift); |
---|
988 | } |
---|
989 | |
---|