started testing a new grid-builder
minor fixes worked on walkers
This commit is contained in:
k-a-z-u
18
grid/Grid.h
18
grid/Grid.h
@@ -177,10 +177,11 @@ public:
|
||||
}
|
||||
|
||||
/** get the center-node the given Point belongs to */
|
||||
const T& getNodeFor(const GridPoint& p) {
|
||||
const UID uid = getUID(p);
|
||||
Assert::isTrue(hashes.find(uid) != hashes.end(), "element not found!");
|
||||
return nodes[hashes[uid]];
|
||||
const T& getNodeFor(const GridPoint& p) const {
|
||||
//const UID uid = getUID(p);
|
||||
auto it = hashes.find(getUID(p));
|
||||
Assert::isTrue(it != hashes.end(), "element not found!");
|
||||
return nodes[it->second];
|
||||
}
|
||||
|
||||
/** get the center-node the given Point belongs to. or nullptr if not present */
|
||||
@@ -249,11 +250,11 @@ public:
|
||||
|
||||
inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
|
||||
inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
|
||||
inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||||
inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||||
|
||||
inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
|
||||
inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
|
||||
inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||||
inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||||
|
||||
|
||||
/** array access */
|
||||
@@ -295,6 +296,11 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
/** convert to a GridPoint coordinate (in cm) */
|
||||
GridPoint toGridPoint(const Point3 pos_m) const {
|
||||
return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) );
|
||||
}
|
||||
|
||||
/** remove the given array-index by moving all follwing elements down by one */
|
||||
template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
|
||||
for (int i = idxToRemove+1; i < arrayLen; ++i) {
|
||||
|
||||
473
grid/factory/v3/GridFactory3.h
Normal file
473
grid/factory/v3/GridFactory3.h
Normal file
@@ -0,0 +1,473 @@
|
||||
#ifndef GRIDFACTORY3_H
|
||||
#define GRIDFACTORY3_H
|
||||
|
||||
#include "../../Grid.h"
|
||||
#include "../../../floorplan/v2/Floorplan.h"
|
||||
#include "HelperPoly3.h"
|
||||
|
||||
|
||||
template <typename Node> class GridFactory3 {
|
||||
|
||||
private:
|
||||
|
||||
|
||||
Grid<Node>& grid;
|
||||
const int gs_cm;
|
||||
|
||||
struct NewNode {
|
||||
GridPoint pos;
|
||||
int type;
|
||||
NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;}
|
||||
bool operator == (const NewNode& o) const {return o.pos == pos;}
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
GridFactory3(Grid<Node>& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) {
|
||||
|
||||
}
|
||||
|
||||
void build(const Floorplan::IndoorMap* map) {
|
||||
|
||||
std::vector<NewNode> add;
|
||||
std::vector<NewNode> rem;
|
||||
|
||||
for (const Floorplan::Floor* floor : map->floors) {
|
||||
|
||||
// for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||||
|
||||
// const std::vector<NewNode> pts = getPointsOn(floor, *poly);
|
||||
// if (poly->method == Floorplan::OutlineMethod::ADD) {
|
||||
// add.insert(add.end(), pts.begin(), pts.end());
|
||||
// } else {
|
||||
// rem.insert(rem.end(), pts.begin(), pts.end());
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
const std::vector<NewNode> pts = getPointsOn(floor);
|
||||
add.insert(add.end(), pts.begin(), pts.end());
|
||||
|
||||
for (const Floorplan::Stair* stair : floor->stairs) {
|
||||
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
|
||||
const std::vector<NewNode> pts = getPointsOn(floor, quads);
|
||||
add.insert(add.end(), pts.begin(), pts.end());
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
for (const NewNode& nn : add) {
|
||||
auto it = std::find(rem.begin(), rem.end(), nn);
|
||||
if (it == rem.end()) {
|
||||
if (!grid.hasNodeFor(nn.pos)) {
|
||||
Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm);
|
||||
n.setType(nn.type);
|
||||
grid.add(n);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
connect(map);
|
||||
removeIsolatedNodes();
|
||||
|
||||
}
|
||||
|
||||
bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) {
|
||||
|
||||
Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy());
|
||||
|
||||
for (Floorplan::Floor* floor : map->floors) {
|
||||
|
||||
if (n1.inMeter().z != floor->atHeight) {continue;}
|
||||
if (n2.inMeter().z != floor->atHeight) {continue;}
|
||||
|
||||
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
||||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
if (line) {
|
||||
const std::vector<Line2> lines = getThickLines(line);
|
||||
for (const Line2& lObs : lines) {
|
||||
if (lObs.getSegmentIntersection(lNodes)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
|
||||
static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
|
||||
//const Line2 base(line->from*100, line->to*100);
|
||||
const float thickness_m = line->thickness_m;
|
||||
const Point2 dir = (line->to - line->from); // obstacle's direction
|
||||
const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree)
|
||||
const Point2 p1 = line->from + perp * thickness_m/2; // start-up
|
||||
const Point2 p2 = line->from - perp * thickness_m/2; // start-down
|
||||
const Point2 p3 = line->to + perp * thickness_m/2; // end-up
|
||||
const Point2 p4 = line->to - perp * thickness_m/2; // end-down
|
||||
return {
|
||||
Line2(p1, p2),
|
||||
Line2(p3, p4),
|
||||
Line2(p2, p4),
|
||||
Line2(p1, p3),
|
||||
};
|
||||
}
|
||||
|
||||
void connect(const Floorplan::IndoorMap* map) {
|
||||
|
||||
for (Node& n1 : grid) {
|
||||
for (Node& n2 : grid) {
|
||||
|
||||
if (n1 == n2) {continue;}
|
||||
|
||||
|
||||
if (
|
||||
(n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) ||
|
||||
(n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR)
|
||||
) {
|
||||
|
||||
const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
|
||||
const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
|
||||
if (distxy > 0 && distxy < gs_cm * 1.5 / 100.0f && distz_cm < gs_cm) {
|
||||
if (n1.fullyConnected()) {continue;}
|
||||
if (n2.fullyConnected()) {continue;}
|
||||
grid.connectUniDir(n1, n2);
|
||||
}
|
||||
|
||||
} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
|
||||
if (n1.getDistanceInCM(n2) < gs_cm * 1.45 && !isBlocked(map, n1, n2)) {
|
||||
if (n1.fullyConnected()) {continue;}
|
||||
if (n2.fullyConnected()) {continue;}
|
||||
grid.connectUniDir(n1, n2);
|
||||
}
|
||||
} else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) {
|
||||
|
||||
const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
|
||||
const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
|
||||
|
||||
// if (n1.getDistanceInCM(n2) < gs_cm * 1.45 && !isBlocked(map, n1, n2)) {
|
||||
if (distxy < gs_cm * 1.45 / 100.0f && distz_cm <= gs_cm) {
|
||||
if (n1.fullyConnected()) {continue;}
|
||||
if (n2.fullyConnected()) {continue;}
|
||||
grid.connectUniDir(n1, n2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// if (n1.getDistanceInCM(n2) < gs_cm * 1.7 && !isBlocked(map, n1, n2)) {
|
||||
// if (n1.fullyConnected()) {continue;}
|
||||
// if (n2.fullyConnected()) {continue;}
|
||||
// grid.connectUniDir(n1, n2);
|
||||
// }
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/** recursively get all connected nodes and add them to the set */
|
||||
void getConnected(Node& n1, std::unordered_set<int>& visited) {
|
||||
|
||||
std::unordered_set<int> toVisit;
|
||||
toVisit.insert(n1.getIdx());
|
||||
|
||||
// run while there are new nodes to visit
|
||||
while(!toVisit.empty()) {
|
||||
|
||||
// get the next node
|
||||
int nextIdx = *toVisit.begin();
|
||||
toVisit.erase(nextIdx);
|
||||
visited.insert(nextIdx);
|
||||
Node& next = grid[nextIdx];
|
||||
|
||||
// get all his (unprocessed) neighbors and add them to the region
|
||||
for (const Node& n2 : grid.neighbors(next)) {
|
||||
if (visited.find(n2.getIdx()) == visited.end()) {
|
||||
toVisit.insert(n2.getIdx());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void removeIsolatedNodes() {
|
||||
|
||||
//std::cout << "todo: remove" << std::endl;
|
||||
//return;
|
||||
|
||||
// try to start at the first stair
|
||||
for (Node& n : grid) {
|
||||
if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
|
||||
}
|
||||
|
||||
// no stair found? try to start at the first node
|
||||
removeIsolatedNodes(grid[0]);
|
||||
|
||||
}
|
||||
|
||||
/** remove all nodes not connected to n1 */
|
||||
void removeIsolatedNodes(Node& n1) {
|
||||
|
||||
// get the connected region around n1
|
||||
//Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
|
||||
//Log::tick();
|
||||
std::unordered_set<int> set;
|
||||
getConnected(n1, set);
|
||||
//Log::tock();
|
||||
|
||||
//const int numToRemove = grid.getNumNodes() - set.size();
|
||||
//int numRemoved = 0;
|
||||
|
||||
// remove all other
|
||||
//Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
|
||||
//Log::tick();
|
||||
for (Node& n2 : grid) {
|
||||
if (set.find(n2.getIdx()) == set.end()) {
|
||||
|
||||
// sanity check
|
||||
// wouldn't make sense that a stair-node is removed..
|
||||
// maybe something went wrong elsewhere???
|
||||
Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
|
||||
Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
|
||||
//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");
|
||||
|
||||
// proceed ;)
|
||||
grid.remove(n2);
|
||||
|
||||
//++numRemoved;
|
||||
//std::cout << numRemoved << ":" << numToRemove << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
//Log::tock();
|
||||
|
||||
// clean the grid (physically delete the removed nodes)
|
||||
grid.cleanup();
|
||||
|
||||
}
|
||||
|
||||
|
||||
// std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) {
|
||||
|
||||
// std::vector<NewNode> res;
|
||||
|
||||
// BBox2 bbox;
|
||||
// for (Point2 pt : poly.poly.points) {bbox.add(pt);}
|
||||
|
||||
// int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||||
// int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||||
|
||||
// int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||||
// int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||||
|
||||
// int z = floor->atHeight * 100;
|
||||
|
||||
// for (int y = y1; y <= y2; y += gs_cm) {
|
||||
// for (int x = x1; x <= x2; x += gs_cm) {
|
||||
|
||||
// GridPoint gp(x, y, z);
|
||||
// int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR;
|
||||
// res.push_back(NewNode(gp, type));
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
// return res;
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor) {
|
||||
|
||||
std::vector<NewNode> res;
|
||||
|
||||
BBox2 bbox;
|
||||
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||||
for (Point2 pt : poly->poly.points) {bbox.add(pt);}
|
||||
}
|
||||
|
||||
int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||||
int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||||
|
||||
int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||||
int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||||
|
||||
int z = floor->atHeight * 100;
|
||||
|
||||
struct Combo {
|
||||
HelperPoly3 poly;
|
||||
const Floorplan::FloorOutlinePolygon* orig;
|
||||
Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;}
|
||||
};
|
||||
|
||||
std::vector<Combo> polygons;
|
||||
for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
||||
HelperPoly3 pol(*poly);
|
||||
polygons.push_back(Combo(pol, poly));
|
||||
}
|
||||
|
||||
for (int y = y1; y <= y2; y += gs_cm) {
|
||||
for (int x = x1; x <= x2; x += gs_cm) {
|
||||
|
||||
int type = GridNode::TYPE_FLOOR;
|
||||
bool remove = false;
|
||||
bool add = false;
|
||||
|
||||
for (const Combo& c : polygons) {
|
||||
if (c.poly.contains(Point2(x,y))) {
|
||||
if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;}
|
||||
if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;}
|
||||
if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;}
|
||||
}
|
||||
}
|
||||
|
||||
if (add && !remove) {
|
||||
GridPoint gp(x, y, z);
|
||||
res.push_back(NewNode(gp, type));
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
//
|
||||
|
||||
// const std::vector<NewNode> pts = getPointsOn(floor, *poly);
|
||||
// if (poly->method == Floorplan::OutlineMethod::ADD) {
|
||||
// add.insert(add.end(), pts.begin(), pts.end());
|
||||
// } else {
|
||||
// rem.insert(rem.end(), pts.begin(), pts.end());
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
|
||||
const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
|
||||
double d00 = dot(v0, v0);
|
||||
double d01 = dot(v0, v1);
|
||||
double d11 = dot(v1, v1);
|
||||
double d20 = dot(v2, v0);
|
||||
double d21 = dot(v2, v1);
|
||||
double denom = d00 * d11 - d01 * d01;
|
||||
v = (d11 * d20 - d01 * d21) / denom;
|
||||
w = (d00 * d21 - d01 * d20) / denom;
|
||||
u = 1.0f - v - w;
|
||||
return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
|
||||
}
|
||||
|
||||
// void isBlocked(const GridPoint& gp) {
|
||||
// for (Floorplan::Floor* floor : map->floors) {
|
||||
// for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
||||
// Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
// if (line) {
|
||||
// line->
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const std::vector<Floorplan::Quad3>& quads) {
|
||||
|
||||
std::vector<NewNode> res;
|
||||
|
||||
// whole stair
|
||||
BBox3 bboxStair;
|
||||
for (const Floorplan::Quad3& quad : quads) {
|
||||
bboxStair.add(quad.p1);
|
||||
bboxStair.add(quad.p2);
|
||||
bboxStair.add(quad.p3);
|
||||
bboxStair.add(quad.p4);
|
||||
}
|
||||
|
||||
// stair's starting and ending z (must be connected to a floor)
|
||||
//int z1 = grid.snapZ( (floor->atHeight) * 100 );
|
||||
//
|
||||
int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 );
|
||||
|
||||
// one quad
|
||||
for (const Floorplan::Quad3& quad : quads) {
|
||||
|
||||
BBox3 bbox;
|
||||
bbox.add(quad.p1);
|
||||
bbox.add(quad.p2);
|
||||
bbox.add(quad.p3);
|
||||
bbox.add(quad.p4);
|
||||
|
||||
|
||||
int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
|
||||
int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;
|
||||
|
||||
int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
|
||||
int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;
|
||||
|
||||
//int zFloor = floor->atHeight * 100;
|
||||
|
||||
for (int y = y1; y <= y2; y += gs_cm) {
|
||||
for (int x = x1; x <= x2; x += gs_cm) {
|
||||
|
||||
int z = 0;
|
||||
Point2 p(x/100.0f, y/100.0f);
|
||||
|
||||
float u,v,w;
|
||||
if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) {
|
||||
z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100;
|
||||
} else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) {
|
||||
z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
|
||||
} else {
|
||||
// outside of the quad -> skip
|
||||
//z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
|
||||
continue;
|
||||
|
||||
//z = zFloor + (
|
||||
// (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w)
|
||||
// ) * 100;
|
||||
|
||||
}
|
||||
|
||||
//z = grid.snapZ(z);
|
||||
|
||||
const GridPoint gp(x, y, z);
|
||||
const int type = GridNode::TYPE_STAIR;
|
||||
res.push_back(NewNode(gp, type));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// scale to ensure starting at floor, and ending at floor
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // GRIDFACTORY3_H
|
||||
101
grid/factory/v3/HelperPoly3.h
Normal file
101
grid/factory/v3/HelperPoly3.h
Normal file
@@ -0,0 +1,101 @@
|
||||
#ifndef HELPERPOLY3_H
|
||||
#define HELPERPOLY3_H
|
||||
|
||||
#include "../../../geo/Point2.h"
|
||||
#include "../../../geo/Point3.h"
|
||||
#include "../../../geo/BBox2.h"
|
||||
#include "../../../geo/BBox3.h"
|
||||
#include "../../../floorplan/v2/Floorplan.h"
|
||||
#include "../../../grid/Grid.h"
|
||||
|
||||
/** helper class for polygon methods */
|
||||
struct HelperPoly3 {
|
||||
|
||||
BBox2 bbox_cm;
|
||||
std::vector<Point2> points_cm;
|
||||
|
||||
/** empty ctor */
|
||||
HelperPoly3() {
|
||||
;
|
||||
}
|
||||
|
||||
/** ctor from floorplan-polygon */
|
||||
HelperPoly3(const Floorplan::FloorOutlinePolygon& poly) {
|
||||
for (Point2 p : poly.poly.points) { add(p * 100); }
|
||||
}
|
||||
|
||||
/** ctor from floorplan-quad */
|
||||
HelperPoly3(const Floorplan::Quad3& quad) {
|
||||
add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
|
||||
}
|
||||
|
||||
/** ctor from floorplan-polygon */
|
||||
HelperPoly3(const Floorplan::Polygon2& poly) {
|
||||
for (Point2 p : poly.points) { add(p * 100); }
|
||||
}
|
||||
|
||||
void add(const Point2 p) {
|
||||
points_cm.push_back(p);
|
||||
bbox_cm.add(p);
|
||||
}
|
||||
|
||||
void add(const Point3& p) {
|
||||
points_cm.push_back(p.xy());
|
||||
bbox_cm.add(p.xy());
|
||||
}
|
||||
|
||||
/** does the polygon contain the given point (in cm)? */
|
||||
bool contains(const Point2 p_cm) const {
|
||||
|
||||
// not within bbox? -> not within polygon
|
||||
if (!bbox_cm.contains(p_cm)) {return false;}
|
||||
|
||||
// ensure the point is at least a bit outside of the polygon
|
||||
const float x1_cm = bbox_cm.getMin().x - 17.71920;
|
||||
const float y1_cm = bbox_cm.getMin().y - 23.10923891;
|
||||
|
||||
// construct line between point outside of the polygon and the point in question
|
||||
const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);
|
||||
|
||||
// determine the number of intersections
|
||||
int hits = 0;
|
||||
const int cnt = points_cm.size();
|
||||
for (int i = 0; i < cnt; ++i) {
|
||||
const Point2 p1 = points_cm[(i+0)%cnt];
|
||||
const Point2 p2 = points_cm[(i+1)%cnt];
|
||||
const Line2 l12(p1, p2);
|
||||
if (l12.getSegmentIntersection(l)) {++hits;}
|
||||
}
|
||||
|
||||
// inside or outside?
|
||||
return ((hits % 2) == 1);
|
||||
|
||||
}
|
||||
|
||||
/** call a user-function for each GRID-ALIGNED point within the polygon */
|
||||
void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
|
||||
|
||||
int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
|
||||
int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
|
||||
|
||||
int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
|
||||
int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
|
||||
|
||||
// process each point within the (aligned) bbox
|
||||
for (int y = y1; y <= y2; y += gridSize_cm) {
|
||||
for (int x = x1; x <= x2; x += gridSize_cm) {
|
||||
|
||||
// does this point belong to the polygon?
|
||||
if (!contains(Point2(x,y))) {continue;}
|
||||
|
||||
// call the callback
|
||||
callback(x,y);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // HELPERPOLY3_H
|
||||
@@ -174,10 +174,11 @@ namespace GW3 {
|
||||
|
||||
public:
|
||||
|
||||
static GridPoint p3ToGp(const Point3 p) {
|
||||
const Point3 p100 = p*100;
|
||||
return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
|
||||
}
|
||||
// static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
|
||||
// const Point3 p100 = p*100;
|
||||
// //return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
|
||||
// return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(p100.z) );
|
||||
// }
|
||||
|
||||
static Point3 gpToP3(const GridPoint gp) {
|
||||
return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
|
||||
|
||||
@@ -4,18 +4,55 @@
|
||||
#include "../../../geo/Heading.h"
|
||||
#include "../../../geo/Point3.h"
|
||||
#include <vector>
|
||||
#include "../../../math/Distributions.h"
|
||||
#include "../../../grid/Grid.h"
|
||||
|
||||
namespace GW3 {
|
||||
|
||||
struct StepSizes {
|
||||
|
||||
float stepSizeFloor_m = NAN;
|
||||
float stepSizeStair_m = NAN;
|
||||
|
||||
bool isValid() const {
|
||||
return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
|
||||
}
|
||||
|
||||
template <typename Node> float inMeter(const int steps, const Point3 start, const Grid<Node>& grid) const {
|
||||
|
||||
Assert::isTrue(isValid(), "invalid step-sizes given");
|
||||
|
||||
const GridPoint gp = grid.toGridPoint(start);
|
||||
const Node& n = grid.getNodeFor(gp);
|
||||
if (grid.isPlain(n)) {
|
||||
return stepSizeFloor_m * steps;
|
||||
} else {
|
||||
return stepSizeStair_m * steps;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** paremters for the walk */
|
||||
struct WalkParams {
|
||||
|
||||
//Distribution::Normal<float> dDistFloor;
|
||||
//Distribution::Normal<float> dDistStair;
|
||||
|
||||
Point3 start;
|
||||
float distance_m;
|
||||
//float distance_m;
|
||||
int numSteps;
|
||||
Heading heading = Heading(0);
|
||||
|
||||
float lookFurther_m = 1.5;
|
||||
|
||||
StepSizes stepSizes;
|
||||
|
||||
template <typename Node> float getDistanceInMeter(const Grid<Node>& grid) const {
|
||||
return stepSizes.inMeter(numSteps, start, grid);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** result of the random walk */
|
||||
|
||||
@@ -9,6 +9,29 @@
|
||||
|
||||
namespace GW3 {
|
||||
|
||||
/** describes a potential walk, which can be evaluated */
|
||||
struct PotentialWalk {
|
||||
|
||||
/** initial parameters (requested walk) */
|
||||
const WalkParams& params;
|
||||
|
||||
/** walk started here */
|
||||
Point3 pStart;
|
||||
|
||||
/** walk ended here */
|
||||
Point3 pEnd;
|
||||
|
||||
/** usually the euclidean distance start<->end but not necessarily! */
|
||||
float walkDist_m;
|
||||
|
||||
/** ctor */
|
||||
PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) :
|
||||
params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) {
|
||||
;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** interface for all evaluators that return a probability for a given walk */
|
||||
template <typename Node> class WalkEvaluator {
|
||||
|
||||
@@ -17,7 +40,7 @@ namespace GW3 {
|
||||
/** get the probability for the given walk */
|
||||
//virtual double getProbability(const Walk<Node>& walk) const = 0;
|
||||
|
||||
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0;
|
||||
virtual double getProbability(const PotentialWalk& walk) const = 0;
|
||||
|
||||
};
|
||||
|
||||
@@ -31,12 +54,9 @@ namespace GW3 {
|
||||
|
||||
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
|
||||
|
||||
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {
|
||||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||||
|
||||
(void) params;
|
||||
(void) pStart;
|
||||
|
||||
const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
|
||||
const GridPoint gp = Helper<Node>::p3ToGp(walk.pEnd);
|
||||
const Node& node = grid->getNodeFor(gp);
|
||||
const double p = node.getWalkImportance();
|
||||
return p;
|
||||
@@ -65,17 +85,15 @@ namespace GW3 {
|
||||
;
|
||||
}
|
||||
|
||||
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
|
||||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||||
|
||||
(void) params;
|
||||
|
||||
if (pStart == pEnd) {
|
||||
if (walk.pStart == walk.pEnd) {
|
||||
std::cout << "warn! start-position == end-positon" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
const Heading head(pStart.xy(), pEnd.xy());
|
||||
const float diff = head.getDiffHalfRAD(params.heading);
|
||||
const Heading head(walk.pStart.xy(), walk.pEnd.xy());
|
||||
const float diff = head.getDiffHalfRAD(walk.params.heading);
|
||||
//const float diff = Heading::getSignedDiff(params.heading, head);
|
||||
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
|
||||
return dist.getProbability(diff);
|
||||
@@ -87,18 +105,20 @@ namespace GW3 {
|
||||
/** evaluate the difference between distance(start, end) and the requested distance */
|
||||
template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
|
||||
|
||||
const Grid<Node>& grid;
|
||||
|
||||
const double sigma;
|
||||
|
||||
const Distribution::Normal<double> dist;
|
||||
|
||||
public:
|
||||
|
||||
WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
|
||||
WalkEvalDistance(const Grid<Node>& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;}
|
||||
|
||||
virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {
|
||||
virtual double getProbability(const PotentialWalk& walk) const override {
|
||||
|
||||
const float requestedDistance_m = params.distance_m;
|
||||
const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
|
||||
const float requestedDistance_m = walk.params.getDistanceInMeter(grid);
|
||||
const float walkedDistance_m = walk.walkDist_m;//pStart.getDistance(pEnd);
|
||||
const float diff = walkedDistance_m - requestedDistance_m;
|
||||
return dist.getProbability(diff);
|
||||
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
|
||||
|
||||
@@ -58,18 +58,22 @@ namespace GW3 {
|
||||
/** perform the walk based on the configured setup */
|
||||
const WalkResult getDestination(const WalkParams& params) const override {
|
||||
|
||||
Assert::isNot0(params.distance_m, "walking distance must be > 0");
|
||||
Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
|
||||
|
||||
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
|
||||
|
||||
|
||||
static std::mt19937 rndGen;
|
||||
|
||||
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
||||
const GridPoint gpStart = grid.toGridPoint(params.start);
|
||||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||
|
||||
// calculate a walk's probability
|
||||
auto getP = [&] (const Point3 dst) {
|
||||
double p = 1;
|
||||
const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
|
||||
for (const WalkEvaluator<Node>* eval : evals) {
|
||||
const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params);
|
||||
const double p1 = eval->getProbability(pWalk);
|
||||
p *= p1;
|
||||
}
|
||||
return p;
|
||||
@@ -97,7 +101,7 @@ namespace GW3 {
|
||||
return (startNode->getDistanceInMeter(n)) < maxDist_m;
|
||||
}
|
||||
};
|
||||
Cond cond(params.distance_m+secBuffer_m, startNode);
|
||||
Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
|
||||
std::vector<const Node*> reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
|
||||
|
||||
WalkResult res;
|
||||
@@ -106,7 +110,7 @@ namespace GW3 {
|
||||
|
||||
// get the to-be-reached destination's position (using start+distance+heading)
|
||||
const Point2 dir = res.heading.asVector();
|
||||
const Point2 dst = params.start.xy() + (dir * params.distance_m);
|
||||
const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));
|
||||
|
||||
// is above destination reachable?
|
||||
const Node* n = Helper::contains(grid, reachableNodes, dst);
|
||||
@@ -114,14 +118,14 @@ namespace GW3 {
|
||||
if (n) {
|
||||
|
||||
const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
|
||||
const GridPoint gp = Helper::p3ToGp(p3);
|
||||
const GridPoint gp = grid.toGridPoint(p3);
|
||||
|
||||
|
||||
|
||||
if (grid.hasNodeFor(gp)) {
|
||||
res.position = p3; // update position
|
||||
//res.heading; // keep as-is
|
||||
res.probability *= 1;//getP(p3); // keep as-is
|
||||
res.probability *= getP(p3); // keep as-is
|
||||
return res; // done
|
||||
|
||||
} else {
|
||||
@@ -145,9 +149,11 @@ namespace GW3 {
|
||||
const Point3 start = params.start;
|
||||
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
|
||||
|
||||
const PotentialWalk pWalk(params, start, end, start.getDistance(end));
|
||||
|
||||
double p = 1;
|
||||
for (const WalkEvaluator<Node>* eval : evals) {
|
||||
const double p1 = eval->getProbability(start, end, start.getDistance(end), params);
|
||||
const double p1 = eval->getProbability(pWalk);
|
||||
p *= p1;
|
||||
}
|
||||
|
||||
@@ -157,8 +163,29 @@ namespace GW3 {
|
||||
}
|
||||
|
||||
res.heading = Heading(start.xy(), end.xy());
|
||||
res.probability *= 0.1;//getP(end);
|
||||
res.probability *= getP(end);
|
||||
res.position = end;
|
||||
|
||||
if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {
|
||||
|
||||
std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;
|
||||
|
||||
std::cout << "issue:" << res.position.asString() << std::endl;
|
||||
|
||||
for (int i = -80; i <= +80; i+=10) {
|
||||
Point3 pos = res.position + Point3(0,0,i/100.0f);
|
||||
std::cout << pos.asString() << " ----- " << res.position.asString() << std::endl;
|
||||
std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
|
||||
std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
int i = 0; (void) i;
|
||||
|
||||
}
|
||||
|
||||
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
@@ -204,14 +231,16 @@ namespace GW3 {
|
||||
/** perform the walk based on the configured setup */
|
||||
const WalkResult getDestination(const WalkParams& params) const override {
|
||||
|
||||
Assert::isNot0(params.distance_m, "walking distance must be > 0");
|
||||
const float walkDist_m = params.getDistanceInMeter(grid);
|
||||
|
||||
Assert::isNot0(walkDist_m, "walking distance must be > 0");
|
||||
|
||||
const GridPoint gpStart = Helper::p3ToGp(params.start);
|
||||
const Node* startNode = grid.getNodePtrFor(gpStart);
|
||||
if (!startNode) {throw Exception("start node not found!");}
|
||||
|
||||
const float maxDist = params.distance_m + gridSize_m;
|
||||
const int depth = std::ceil(params.distance_m / gridSize_m) + 1;
|
||||
const float maxDist = walkDist_m + gridSize_m;
|
||||
const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
|
||||
|
||||
Point3 best; double bestP = 0;
|
||||
//DrawList<Point3> drawer;
|
||||
@@ -331,9 +360,11 @@ namespace GW3 {
|
||||
//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
|
||||
const float walkDist_m = end.getDistance(start);//*1.05;
|
||||
|
||||
const PotentialWalk pWalk(params, start, end, walkDist_m);
|
||||
|
||||
double p = 1;
|
||||
for (const WalkEvaluator<Node>* eval : evals) {
|
||||
const double p1 = eval->getProbability(start, end, walkDist_m, params);
|
||||
const double p1 = eval->getProbability(pWalk);
|
||||
p *= p1;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user