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

started removing KLib related code:

Browse Source 
- assertions
- distributions
new helper methods
worked on stairs
worked on grid-walkers
worked on navigation
This commit is contained in:
FrankE
2016-01-27 20:03:58 +01:00
parent e6329e1db4
commit 0e05f4bef8
26 changed files with 408 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

176
grid/factory/GridFactory.h
View File

@@ -6,6 +6,7 @@
#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.h"
#include "../../floorplan/PlatformStair.h"
#include "../../geo/Units.h"
#include "../GridNodeBBox.h"
@@ -57,40 +58,32 @@ public:
}
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {
for (const int idx : indices) {
// connect the node with its neighbors
connectAdjacent(grid[idx]);
}
}
/** connect all neighboring nodes located on the given height-plane */
void connectAdjacent(const float z_cm) {
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
Log::tick();
const int gridSize_cm = grid.getGridSize_cm();
// connect adjacent grid-points
for (T& n1 : grid) {
// not the floor we are looking for? -> skip (ugly.. slow(er))
if (n1.z_cm != z_cm) {continue;}
// square around each point
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
const int ox = n1.x_cm + x;
const int oy = n1.y_cm + y;
const GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
}
}
// connect the node with its neighbors
connectAdjacent(n1);
}
@@ -98,54 +91,124 @@ public:
}
/** connect the given node with its neighbors */
void connectAdjacent(T& n1) {
const int gridSize_cm = grid.getGridSize_cm();
// square around the node
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
// skip the center (node itself)
if ((x == y) && (x == 0)) {continue;}
// position of the potential neighbor
const int ox = n1.x_cm + x;
const int oy = n1.y_cm + y;
const GridPoint p(ox, oy, n1.z_cm);
// does the grid contain the potential neighbor?
const T* n2 = grid.getNodePtrFor(p);
if (n2 != nullptr) {
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
}
}
}
}
/** shrink the given bbox to be grid-aligned */
BBox2 shrinkAlign(const BBox2& bb) {
const float gridSize_cm = grid.getGridSize_cm();
Point2 p1 = bb.getMin();
Point2 p2 = bb.getMax();
p1.x = std::ceil(p1.x/gridSize_cm)*gridSize_cm;
p1.y = std::ceil(p1.y/gridSize_cm)*gridSize_cm;
p2.x = std::floor(p2.x/gridSize_cm)*gridSize_cm;
p2.y = std::floor(p2.y/gridSize_cm)*gridSize_cm;
BBox2 res; res.add(p1); res.add(p2); return res;
}
/** add a new platform-stair between the two given floors */
void buildPlatformStair(const PlatformStair& s, const float z1_cm, const float z2_cm) {
const float zCenter_cm = (z2_cm + z1_cm) / 2;
std::vector<int> indices;
// add the platform in the middle
BBox2 bb = shrinkAlign(s.platform);
const int gridSize_cm = grid.getGridSize_cm();
for (int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
for (int y_cm = bb.getMin().y; y_cm <= bb.getMax().y; y_cm += gridSize_cm) {
int idx = grid.add(T(x_cm, y_cm, zCenter_cm));
indices.push_back(idx);
}
}
// connect the plattform in the middle
connectAdjacent(indices);
// TODO: interconnect (x-y) the stair lines???
buildStair(s.s1, z1_cm, zCenter_cm);
buildStair(s.s2, z2_cm, zCenter_cm);
}
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
Log::tick();
for (const Stair& s : stairs) {
// potential starting-point for the stair
for (T& n : grid) {
// node lies on the stair's starting edge?
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grap? -> construct stair
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStair(n, n2);
}
}
}
}
for (const Stair& s : stairs) { buildStair(s, z1_cm, z2_cm); }
Log::tock();
}
/** build a stair (z-transition) from n1 to n2 */
void buildStair(T& n1, T& n2) {
void buildStair(const Stair& s, const float z1_cm, const float z2_cm) {
// potential starting-point for the stair
for (T& n : grid) {
// node lies on the stair's starting edge?
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
// construct end-point by using the stair's direction
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
GridPoint gp(end.x, end.y, end.z);
// does such and end-point exist within the grid? -> construct stair-line
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStairLine(n, n2);
}
}
}
}
/** build a stair (z-transition) from n1 to n2 */
void buildStairLine(T& _n1, T& _n2) {
const int gridSize_cm = grid.getGridSize_cm();
// local copies, needed for std::swap to work
T n1 = _n1; T n2 = _n2;
// ensure we work from lower to upper levels
if (n2.z_cm < n1.z_cm) { std::swap(n1, n2); }
//TODO: ensure n1 is below n2
const float zDiff = n2.z_cm - n1.z_cm;
const float xDiff = n2.x_cm - n1.x_cm;
const float yDiff = n2.y_cm - n1.y_cm;
int idx1 = n1.getIdx();
int idx2 = -1;
const int idx3 = n2.getIdx();
const int gridSize_cm = grid.getGridSize_cm();
int idx1 = n1.getIdx(); // starting node
int idx2 = -1; // next node
const int idx3 = n2.getIdx(); // final node
// move upards in gridSize steps
for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {
@@ -170,9 +233,8 @@ public:
}
// add the last segment
if (idx2 != -1) {
grid.connectBiDir(idx2, idx3);
}
Assert::isTrue(idx2 != -1, "strange stair issue?!");
grid.connectBiDir(idx2, idx3);
}

16
grid/factory/GridImportance.h
View File

@@ -11,7 +11,7 @@
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../nav/dijkstra/DijkstraPath.h"
#include <KLib/math/distribution/Normal.h>
#include "../../math/Distributions.h"
@@ -69,7 +69,7 @@ public:
n1.imp = 1.0f;
n1.imp += getWallImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
n1.imp += getWallImportance( Units::cmToM(std::sqrt(squaredDist[0])) );
//addDoor(n1, neighbors);
// is the current node a door?
@@ -86,7 +86,7 @@ public:
// process each node again
for (T& n1 : g) {
static K::NormalDistribution favorDoors(0.0, 1.0);
static Distribution::Normal<float> favorDoors(0.0f, 1.0f);
// get the distance to the nearest door
const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
@@ -141,7 +141,7 @@ public:
//T& node = g[idx];
const float dist_cm = knn.getNearestDistance( {n.x_cm, n.y_cm, n.z_cm} );
const float dist_m = Units::cmToM(dist_cm);
n.impPath = 1.0 + K::NormalDistribution::getProbability(0, 1.0, dist_m) * 0.8;
n.impPath = 1.0 + Distribution::Normal<float>::getProbability(0, 1.0, dist_m) * 0.8;
}
@@ -191,16 +191,16 @@ public:
}
/** get the importance of the given node depending on its nearest wall */
template <typename T> float getWallImportance(T& nSrc, float dist_m) {
float getWallImportance(float dist_m) {
// avoid sticking too close to walls (unlikely)
static K::NormalDistribution avoidWalls(0.0, 0.4);
static Distribution::Normal<float> avoidWalls(0.0, 0.4);
// favour walking near walls (likely)
static K::NormalDistribution sticToWalls(0.9, 0.5);
static Distribution::Normal<float> sticToWalls(0.9, 0.5);
// favour walking far away (likely)
static K::NormalDistribution farAway(2.2, 0.5);
static Distribution::Normal<float> farAway(2.2, 0.5);
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance