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