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