FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added very simple particle-trap detection

Browse Source
This commit is contained in:
FrankE
2016-09-29 21:39:19 +02:00
parent e75327090d
commit 4a0442f511
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
grid/walk/v2/GridWalker.h
View File

@@ -5,6 +5,7 @@
#include "../../Grid.h" #include "../../Grid.h"
#include "../../../math/DrawList.h" #include "../../../math/DrawList.h"
#include "modules/WalkModule.h" #include "modules/WalkModule.h"
#include "../../../math/Distributions.h"
/** /**
* modular grid-walker that takes various sub-components to determine * modular grid-walker that takes various sub-components to determine
@@ -27,10 +28,10 @@ public:
} }
/** perform the walk based on the configured setup */ /** perform the walk based on the configured setup */
WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m, double& probability) { WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float _dist_m, double& probability) {
Assert::isTrue(dist_m >= 0, "walk distance must not be negative!"); Assert::isTrue(_dist_m >= 0, "walk distance must not be negative!");
Assert::isTrue(dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!"); Assert::isTrue(_dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");
// keep the starting state for reference // keep the starting state for reference
//const WalkState startState = _startState; //const WalkState startState = _startState;
@@ -50,7 +51,7 @@ public:
// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced) // add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
dist_m += _startState.distance.error_m; float dist_m = _dist_m + _startState.distance.error_m;
probability = 1.0; probability = 1.0;
int cnt = 0; int cnt = 0;
@@ -90,7 +91,7 @@ public:
} }
//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;} //if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
probability = curNode->getWalkImportance(); probability = curNode->getWalkImportance() < 0.4f ? (1e-5) : (1.0); // "kill" particles that walk near walls (most probably trapped ones)
// update after // update after
updateAfter(currentState, *startNode, *curNode); updateAfter(currentState, *startNode, *curNode);