From c346b7f2229ef7401b3bb3b52ef355ac53d0f7cd Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 20 Dec 2017 17:12:30 +0100 Subject: [PATCH] started testing a new grid-builder minor fixes worked on walkers --- grid/Grid.h | 18 +- grid/factory/v3/GridFactory3.h | 473 +++++++++++++++++++++++++++++++++ grid/factory/v3/HelperPoly3.h | 101 +++++++ grid/walk/v3/Helper.h | 9 +- grid/walk/v3/Structs.h | 39 ++- grid/walk/v3/WalkEvaluator.h | 52 ++-- grid/walk/v3/Walker.h | 57 +++- 7 files changed, 709 insertions(+), 40 deletions(-) create mode 100644 grid/factory/v3/GridFactory3.h create mode 100644 grid/factory/v3/HelperPoly3.h diff --git a/grid/Grid.h b/grid/Grid.h index 259cb52..4340e8b 100755 --- a/grid/Grid.h +++ b/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 void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) { for (int i = idxToRemove+1; i < arrayLen; ++i) { diff --git a/grid/factory/v3/GridFactory3.h b/grid/factory/v3/GridFactory3.h new file mode 100644 index 0000000..cd68c86 --- /dev/null +++ b/grid/factory/v3/GridFactory3.h @@ -0,0 +1,473 @@ +#ifndef GRIDFACTORY3_H +#define GRIDFACTORY3_H + +#include "../../Grid.h" +#include "../../../floorplan/v2/Floorplan.h" +#include "HelperPoly3.h" + + +template class GridFactory3 { + +private: + + + Grid& 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& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) { + + } + + void build(const Floorplan::IndoorMap* map) { + + std::vector add; + std::vector rem; + + for (const Floorplan::Floor* floor : map->floors) { + +// for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) { + +// const std::vector 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 pts = getPointsOn(floor); + add.insert(add.end(), pts.begin(), pts.end()); + + for (const Floorplan::Stair* stair : floor->stairs) { + std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + const std::vector 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(obs); + if (line) { + const std::vector 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 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& visited) { + + std::unordered_set 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 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 getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) { + +// std::vector 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 getPointsOn(const Floorplan::Floor* floor) { + + std::vector 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 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 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(obs); +// if (line) { +// line-> +// } +// } +// } +// } + + std::vector getPointsOn(const Floorplan::Floor* floor, const std::vector& quads) { + + std::vector 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 diff --git a/grid/factory/v3/HelperPoly3.h b/grid/factory/v3/HelperPoly3.h new file mode 100644 index 0000000..652159d --- /dev/null +++ b/grid/factory/v3/HelperPoly3.h @@ -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 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 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 diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index c62e7b8..57b6c13 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.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& 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); diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h index 8d46389..2c92da6 100644 --- a/grid/walk/v3/Structs.h +++ b/grid/walk/v3/Structs.h @@ -4,18 +4,55 @@ #include "../../../geo/Heading.h" #include "../../../geo/Point3.h" #include +#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 float inMeter(const int steps, const Point3 start, const Grid& 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 dDistFloor; + //Distribution::Normal dDistStair; + Point3 start; - float distance_m; + //float distance_m; + int numSteps; Heading heading = Heading(0); float lookFurther_m = 1.5; + StepSizes stepSizes; + + template float getDistanceInMeter(const Grid& grid) const { + return stepSizes.inMeter(numSteps, start, grid); + } + }; /** result of the random walk */ diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index c34080a..340bbb8 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -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 class WalkEvaluator { @@ -17,7 +40,7 @@ namespace GW3 { /** get the probability for the given walk */ //virtual double getProbability(const Walk& 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* 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::p3ToGp(pEnd); + const GridPoint gp = Helper::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::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 class WalkEvalDistance : public WalkEvaluator { + const Grid& grid; + const double sigma; const Distribution::Normal dist; public: - WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} + WalkEvalDistance(const Grid& 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::getProbability(params.distance_m, sigma, walkedDistance_m); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index bee1178..48a3c2e 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -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* 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 reachableNodes = ReachableByConditionUnsorted::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* 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 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* eval : evals) { - const double p1 = eval->getProbability(start, end, walkDist_m, params); + const double p1 = eval->getProbability(pWalk); p *= p1; }