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

merged

Browse Source
This commit is contained in:
toni
2018-01-17 10:26:16 +01:00
parent 74981c6a45 55061ef0da
commit c02d07af51
67 changed files with 16100 additions and 2117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -85,6 +85,7 @@ ADD_DEFINITIONS(
-DWITH_TESTS
-DWITH_ASSERTIONS
-DWITH_DEBUG_LOG
-D_GLIBCXX_DEBUG
)

5
floorplan/v2/Floorplan.h
View File

@@ -130,6 +130,10 @@ namespace Floorplan {
default: throw Exception("out of bounds");
}
}
/** same z-value for all points? */
bool isLeveled() const {
return (p1.z == p2.z) && (p2.z == p3.z) && (p3.z == p4.z);
}
};
/** additional type-info for obstacles */
@@ -202,6 +206,7 @@ namespace Floorplan {
/** describes one floor within the map, starting at a given height */
struct Floor {
bool enabled = true;
float atHeight; // the floor's starting height
float height; // the floor's total height (from start)
std::string name; // the floor's name

1
geo/Angle.h
View File

@@ -4,6 +4,7 @@
#include <cmath>
#include "../Assertions.h"
#include "Point2.h"
#include "../math/speed.h"
#define PI ((float) M_PI)

48
grid/Grid.h
View File

@@ -16,6 +16,11 @@
#include "../geo/BBox3.h"
#include "../misc/Debug.h"
#define GM_BOX 1
#define GM_HOBEYCOMB 2
#define GRID_MODE GM_BOX
/**
* grid of a given-size, storing some user-data-value which
* - extends GridPoint and GridNode
@@ -177,10 +182,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 */
@@ -213,6 +219,14 @@ public:
return GridNodeBBox(node, gridSize_cm);
}
/** is this node part of a non-plain stair/escalator */
bool isPlain(const T& n1) const {
for (const T& n2 : neighbors(n1)) {
if (n2.z_cm != n1.z_cm) {return false;}
}
return true;
}
/**
* get an UID for the given point.
* this works only for aligned points.
@@ -231,9 +245,18 @@ public:
const uint64_t center = 1 << 19;
// build
#if (GRID_MODE == GM_HOBEYCOMB)
const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2);
const uint64_t x = center + (int64_t) idxX(p.x_cm-xx);
const uint64_t y = center + (int64_t) idxY(p.y_cm);
const uint64_t z = center + (int64_t) idxZ(p.z_cm);
#elif (GRID_MODE == GM_BOX)
const uint64_t x = center + (int64_t) idxX(p.x_cm);
const uint64_t y = center + (int64_t) idxY(p.y_cm);
const uint64_t z = center + (int64_t) idxZ(p.z_cm);
#endif
return (z << 40) | (y << 20) | (x << 0);
@@ -241,11 +264,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 */
@@ -287,6 +310,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) {
@@ -469,11 +497,11 @@ public:
NeighborForEach neighbors(const int idx) {
NeighborForEach neighbors(const int idx) const {
return neighbors(nodes[idx]);
}
NeighborForEach neighbors(const T& node) {
NeighborForEach neighbors(const T& node) const {
return NeighborForEach(*this, node._idx);
}
@@ -513,9 +541,13 @@ private:
/** asssert that the given element is aligned to the grid */
void assertAligned(const T& elem) {
#if (GRID_MODE == GM_HOBEYCOMB)
#elif (GRID_MODE == GM_BOX)
if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
#endif
}
};

1
grid/GridNode.h
View File

@@ -87,6 +87,7 @@ public:
/** set the node's semantic type */
void setType(const uint8_t type) {this->_type = type;}
// /** get the n-th neighbor for this node */
// template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
// return grid.getNeighbor(_idx, nth);

492
grid/factory/v3/GridFactory3.h Normal file
View File

@@ -0,0 +1,492 @@
#ifndef GRIDFACTORY3_H
#define GRIDFACTORY3_H
#include "../../Grid.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "HelperPoly3.h"
#include <unordered_set>
#if (GRID_MODE == GM_BOX)
#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) { for (int x = x1; x <= x2; x += gs_cm) {
#elif (GRID_MODE == GM_HOBEYCOMB)
#define GF3_ITER_XY\
for (int y = y1; y <= y2; y += gs_cm) {\
const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\
for (int x = x1-xx; x <= x2; x += gs_cm) {
#endif
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;}
// stair with floor
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.2 / 100.0f && distz_cm < gs_cm) { // [1.85]
if (n1.fullyConnected()) {continue;}
if (n2.fullyConnected()) {continue;}
grid.connectUniDir(n1, n2);
}
// floor with floor
} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
if (n1.getDistanceInCM(n2) < gs_cm * 1.2 && !isBlocked(map, n1, n2)) { // [1.2 | 1.845]
if (n1.fullyConnected()) {continue;}
if (n2.fullyConnected()) {continue;}
grid.connectUniDir(n1, n2);
}
// stair with stair
} 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.2 / 100.0f && distz_cm <= gs_cm) { // [1.845]
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));
}
GF3_ITER_XY
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) {
// const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);
// for (int x = x1-xx; x <= x2; x += gs_cm) {
GF3_ITER_XY
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
View 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

75
grid/walk/v3/Helper.h
View File

@@ -84,43 +84,67 @@ namespace GW3 {
};
/**
* data-structure to track to-be-visited nodes
* push_back, pop_front
* as pop_front is costly, we omit the pop and use a head-index instead
* memory-consumption vs speed
*/
struct ToVisit {
size_t nextIdx = 0;
std::vector<uint32_t> vec;
ToVisit() {vec.reserve(256);}
void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
uint32_t next() {return vec[nextIdx++];}
bool empty() const {return nextIdx >= vec.size();}
};
/** get an iterator over all nodes reachable from the given start */
template <typename Node> class ReachableIteratorUnsorted {
template <typename Node, typename Conditions> class ReachableIteratorUnsorted {
const Grid<Node>& grid;
const Node& start;
Node* curNode = nullptr;
std::unordered_set<uint32_t> visited;
std::vector<uint32_t> toVisit;
ToVisit toVisit;
Conditions cond;
public:
ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
toVisit.push_back(start.getIdx());
ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
toVisit.add(start.getIdx());
}
bool hasNext() const {
return !toVisit.empty();
}
const Node& next(const std::function<bool(const Node&)>& skip) {
const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance)
toVisit.erase(toVisit.begin());
visited.insert(curIdx);
//const Node& next(const std::function<bool(const Node&)>& skip) {
//template <typename Skip> const Node& next(const Skip skip) {
const Node& next() {
// get the next to-be-visited node
const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance)
const Node& curNode = grid[curIdx];
for (int i = 0; i < curNode.getNumNeighbors(); ++i) {
// mark as "visited"
visited.insert(curIdx);
const int neighborIdx = curNode.getNeighborIdx(i);
// get all neighbors
const int numNeighbors = curNode.getNumNeighbors();
for (int i = 0; i < numNeighbors; ++i) {
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
const Node& neighbor = grid[neighborIdx];
const bool visit = cond.visit(neighbor) ;
// not yet reached -> store distance
if (!skip(neighbor)) {
if (visit) {
if (visited.find(neighborIdx) == visited.end()) {
toVisit.push_back(neighborIdx);
toVisit.add(neighborIdx);
}
}
@@ -150,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);
@@ -169,10 +194,22 @@ namespace GW3 {
return bbox.contains(pt);
}
// /** does one of the given grid-nodes contains the provided point-in-question? */
// static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
// for (const Node* n : nodes) {
// if (contains(grid, n, pt)) {
// return n;
// }
// }
// return nullptr;
// }
/** does one of the given grid-nodes contains the provided point-in-question? */
static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
static const Node* contains(const Grid<Node>& grid, const std::vector<const Node*>& nodes, Point2 pt) {
for (const Node* n : nodes) {
if (contains(grid, n, pt)) {return n;}
if (contains(grid, n, pt)) {
return n;
}
}
return nullptr;
}

340
grid/walk/v3/Reachable.h Normal file
View File

@@ -0,0 +1,340 @@
#ifndef INDOOR_GW3_REACHABLE_H
#define INDOOR_GW3_REACHABLE_H
#include <vector>
#include <set>
#include "../../Grid.h"
namespace GW3 {
#define likely(x) __builtin_expect((x),1)
#define unlikely(x) __builtin_expect((x),0)
/**
* get all grid nodes that are reachable within x-edges (depth)
*/
template <typename Node> class ReachableByDepthUnsorted {
struct VisitEntry {
const Node* gn;
int depth;
VisitEntry() {;}
VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
};
struct Visits {
VisitEntry visits[512];// __attribute__((aligned(16)));
size_t head = 0;
size_t tail = 0;
VisitEntry& getNext() {
return visits[tail++];
}
void add(const VisitEntry& e) {
visits[head++] = e;
assert(head < 512);
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
}
bool hasMore() const {
return head > tail;
}
};
const Grid<Node>& grid;
public:
ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
;
}
/** get all nodes reachable from start using maxDepth steps */
std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {
std::unordered_set<const Node*> checked;
// assuming max 8 neighbors per node, we need
// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
// which is 1 + (1+2+3+4+5)*neighbors
// which is 1 + (n*n + n)/2*neighbors
// however this seems to be slow?!
//const int n = maxDepth + 1;
//const int maxEntries = (n * n + n) / 2 * 10 + 1;
//const int toAlloc = 4096 / sizeof(VisitEntry);
//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
//if (maxDepth > 9) {throw Exception("will not fit!");}
Visits toVisit;
// directly start with the node itself and all its neighbors
checked.insert(&start);
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
const int nIdx = start.getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
checked.insert(&gnNext);
toVisit.add(VisitEntry(&gnNext, 1));
}
// check all to-be-visited nodes
while ( likely(toVisit.hasMore()) ) {
const VisitEntry& e = toVisit.getNext();
if ( likely(e.depth <= maxDepth) ) {
const Node* gnCur = e.gn;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
toVisit.add(VisitEntry(&gnNext, e.depth+1));
checked.insert(&gnNext);
}
}
}
}
return checked;
}
};
/**
* get all grid nodes that are reachable within x-edges (depth)
* additionally returns the needed walking distance in meter
*/
template <typename Node> class ReachableByDepthWithDistanceSorted {
struct VisitEntry {
const Node* gn;
int depth;
float dist_m;
int myIdx;
VisitEntry() {;}
VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
};
struct Visits {
VisitEntry visits[1024];// __attribute__((aligned(16)));
size_t head = 0;
size_t tail = 0;
VisitEntry& getNext() {
return visits[tail++];
}
void add(const VisitEntry& e) {
visits[head++] = e;
assert(head < 1024);
//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
}
bool hasMore() const {
return head > tail;
}
void sort() {
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
return e1.dist_m < e2.dist_m;
};
std::sort(&visits[tail], &visits[head], comp);
}
};
const Grid<Node>& grid;
public:
/** result */
struct Entry {
const Node* node;
const float walkDistToStart_m;
const int prevIdx;
Entry(const Node* node, const float dist, const size_t prevIdx) :
node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}
bool hasPrev() const {
return prevIdx >= 0;
}
};
ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
;
}
/** get all nodes reachable from start using maxDepth steps */
std::vector<Entry> get(const Node& start, const int maxDepth) {
std::unordered_set<const Node*> checked;
std::vector<Entry> res;
Visits toVisit;
// directly start with the node itself and all its neighbors
checked.insert(&start);
res.push_back(Entry(&start, 0, -1));
for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
const int nIdx = start.getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
const float dist_m = gnNext.getDistanceInMeter(start);
toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
res.push_back(Entry(&gnNext, dist_m, 0));
checked.insert(&gnNext);
}
toVisit.sort();
// check all to-be-visited nodes
while ( likely(toVisit.hasMore()) ) {
const VisitEntry& e = toVisit.getNext();
if ( likely(e.depth <= maxDepth) ) {
const Node* gnCur = e.gn;
// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
// const int nIdx = gnCur->getNeighborIdx(i);
// const Node& gnNext = grid[nIdx];
// if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
// const float dist_m = e.dist_m + nodeNodeDist_m;
// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
// res.push_back(Entry(&gnNext, dist_m, e.myIdx));
// checked.insert(&gnNext);
// }
// }
// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;
std::vector<VisitEntry> sub;
for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
const int nIdx = gnCur->getNeighborIdx(i);
const Node& gnNext = grid[nIdx];
if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext);
const float dist_m = e.dist_m + nodeNodeDist_m;
//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
res.push_back(Entry(&gnNext, dist_m, e.myIdx));
checked.insert(&gnNext);
}
}
// dijkstra.. sort the new nodes by destination to start
// only sorting the 8 new nodes seems enough due to the graph's layout
const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
return e1.dist_m < e2.dist_m;
};
std::sort(sub.begin(), sub.end(), comp);
for (const VisitEntry& e : sub) {
toVisit.add(e);
}
}
// slower with same result ;)
//toVisit.sort();
}
return res;
}
};
/**
* data-structure to track to-be-visited nodes
* push_back, pop_front
* as pop_front is costly, we omit the pop and use a head-index instead
* memory-consumption vs speed
*/
struct _ToVisit {
size_t nextIdx = 0;
std::vector<uint32_t> vec;
_ToVisit() {vec.reserve(256);}
void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
uint32_t next() {return vec[nextIdx++];}
bool empty() const {return nextIdx >= vec.size();}
};
/** get a list of all nodes that are reachable after checking several conditions */
template <typename Node, typename Conditions> class ReachableByConditionUnsorted {
public:
static std::vector<const Node*> get(const Grid<Node>& grid, const Node& start, const Conditions cond) {
//Node* curNode = nullptr;
std::unordered_set<uint32_t> scheduled;
_ToVisit toVisit;
toVisit.add(start.getIdx());
std::vector<const Node*> res;
while(!toVisit.empty()) {
// get the next to-be-visited node
const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance)
const Node& curNode = grid[curIdx];
// process current node
res.push_back(&curNode);
scheduled.insert(curIdx);
// get all neighbors
const int numNeighbors = curNode.getNumNeighbors();
for (int i = 0; i < numNeighbors; ++i) {
const uint32_t neighborIdx = curNode.getNeighborIdx(i);
// already visited?
if (scheduled.find(neighborIdx) != scheduled.end()) {continue;}
scheduled.insert(neighborIdx);
// matches the used condition?
const Node& neighbor = grid[neighborIdx];
if (!cond.visit(neighbor)) {continue;}
// OK!
toVisit.add(neighborIdx);
}
}
// done
return res;
}
//const Node& next(const std::function<bool(const Node&)>& skip) {
//template <typename Skip> const Node& next(const Skip skip) {
const Node& next() {
}
};
}
#endif // REACHABLE_H

81
grid/walk/v3/ReachableSampler.h Normal file
View File

@@ -0,0 +1,81 @@
#ifndef INDOOR_GW3_REACHABLESAMPLER_H
#define INDOOR_GW3_REACHABLESAMPLER_H
#include "../../../math/Random.h"
#include "Reachable.h"
#include "Helper.h"
namespace GW3 {
template <typename Node> class ReachableSamplerByDepth {
public:
using Entry = typename ReachableByDepthWithDistanceSorted<Node>::Entry;
struct SampleResult {
Point3 pos;
float walkDistToStart_m;
SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;}
};
private:
const Grid<Node>& grid;
const float gridSize_m;
const std::vector<Entry>& reachableNodes;
mutable RandomGenerator gen;
mutable std::uniform_real_distribution<float> dOffset;
public:
/** ctor */
ReachableSamplerByDepth(const Grid<Node>& grid, const std::vector<Entry>& reachableNodes) :
grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) {
;
}
SampleResult sample() {
std::uniform_int_distribution<int> dIdx(0, reachableNodes.size() - 1);
const int idx = dIdx(gen);
const Entry* e = &reachableNodes[idx];
const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]);
const Node* nDst = e->node;
// center of the destination node
const Point3 nodeCenter = Helper<Node>::gpToP3(*nDst);
// random position within destination-node
const float ox = dOffset(gen);
const float oy = dOffset(gen);
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
// calculate end's walking-distance towards the start
float distToStart_m;
if (ePrev1) {
distToStart_m = ePrev1->walkDistToStart_m + (Helper<Node>::gpToP3(*(ePrev1->node)).getDistance(end));
} else {
distToStart_m = nodeCenter.getDistance(end);
}
// done
return SampleResult(end, distToStart_m);
}
};
}
#endif // REACHABLESAMPLER_H

39
grid/walk/v3/Structs.h
View File

@@ -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 */

75
grid/walk/v3/WalkEvaluator.h
View File

@@ -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 WalkParams& params) const = 0;
virtual double getProbability(const PotentialWalk& walk) const = 0;
};
@@ -31,15 +54,13 @@ namespace GW3 {
WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, 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 std::pow(p,10);
return p;
//return std::pow(p,10);
}
@@ -50,25 +71,32 @@ namespace GW3 {
/** evaluate the difference between head(start,end) and the requested heading */
template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
const double sigma;
const double sigma_rad;
const double kappa;
Distribution::VonMises<double> _dist;
Distribution::LUT<double> dist;
public:
WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;}
// kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
;
}
virtual double getProbability(const Point3 pStart, const Point3 pEnd, 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 Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
}
@@ -77,16 +105,23 @@ 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) {;}
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 WalkParams& params) const override {
virtual double getProbability(const PotentialWalk& walk) const override {
const float walkedDistance_m = pStart.getDistance(pEnd);
return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
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);
}

321
grid/walk/v3/Walker.h
View File

@@ -14,6 +14,8 @@
#include "Helper.h"
#include "Structs.h"
#include "WalkEvaluator.h"
#include "Reachable.h"
#include "ReachableSampler.h"
namespace GW3 {
@@ -26,18 +28,22 @@ namespace GW3 {
public:
/** get a new destination for the given start */
virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;
virtual const WalkResult getDestination(const WalkParams& params) const = 0;
};
template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {
//Random::RandomGenerator rnd;
Grid<Node>& grid;
std::vector<WalkEvaluator<Node>*> evals;
public:
/** ctor */
WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
;
}
/** make the code a little more readable */
using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>;
@@ -50,44 +56,76 @@ namespace GW3 {
}
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
const WalkResult getDestination(const WalkParams& params) const override {
Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0");
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");
Assert::isNot0(params.distance_m, "walking distance must be > 0");
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(pWalk);
p *= p1;
}
return p;
};
// include one additional grid-cell (increased distance)
const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
ReachableSettings set;
set.limitDistance = true;
set.dist_m = params.distance_m + secBuffer_m;
set.limitHeading = false;
set.heading = params.heading;
set.maxHeadingDiff_rad = M_PI/2;
const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
//const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1);
const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15);
// ReachableSettings set;
// set.limitDistance = true;
// set.dist_m = params.distance_m + secBuffer_m;
// set.limitHeading = false;
// set.heading = params.heading;
// set.maxHeadingDiff_rad = M_PI/2;
// // get all nodes that satisfy above constraints
// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
struct Cond {
const float maxDist_m;
const Node* startNode;
Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
bool visit(const Node& n) const {
return (startNode->getDistanceInMeter(n)) < maxDist_m;
}
};
Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
std::vector<const Node*> reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);
WalkResult res;
res.heading = params.heading;
res.position = params.start;
// 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 dst reachable?
// is above destination reachable?
const Node* n = Helper::contains(grid, reachableNodes, dst);
//const Node* n = ri.contains(dst);
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; // keep as-is
res.probability *= getP(p3); // keep as-is
return res; // done
} else {
@@ -111,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, params);
const double p1 = eval->getProbability(pWalk);
p *= p1;
}
@@ -123,8 +163,31 @@ namespace GW3 {
}
res.heading = Heading(start.xy(), end.xy());
res.probability = p;
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;
}
#if (GRID_MODE == GM_BOX)
Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
#endif
return res;
}
@@ -142,8 +205,20 @@ namespace GW3 {
std::vector<WalkEvaluator<Node>*> evals;
Grid<Node>& grid;
const float gridSize_m;
mutable std::minstd_rand rndGen;
mutable std::uniform_real_distribution<float> dFinal;
public:
/** ctor */
WalkerWeightedRandom(Grid<Node>& grid) :
grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
;
}
/** make the code a little more readable */
using Helper = GW3::Helper<Node>;
using Walk = typename GW3::Walk<Node>;
@@ -156,101 +231,189 @@ namespace GW3 {
}
/** perform the walk based on the configured setup */
const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {
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);
static std::minstd_rand rndGen;
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!");}
// // include one additional grid-cell (increased distance)
// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);
const float maxDist = walkDist_m + gridSize_m;
const int depth = std::ceil(walkDist_m / gridSize_m) + 1;
// ReachableSettings set;
// set.limitDistance = true;
// set.limitHeading = true;
// set.dist_m = params.distance_m + secBuffer_m;
// set.heading = params.heading;
// set.maxHeadingDiff_rad = M_PI/2;
// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);
const float gridSize_m = grid.getGridSize_cm() / 100.0f;
//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);
Point3 best;
double bestP = 0;
// DrawList<Point3> drawer;
Point3 best; double bestP = 0;
//DrawList<Point3> drawer;
const Point3 start = params.start;
// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
//for (int i = 0; i < 500; ++i) {
// const Node* dstNode = reachableNodes[dNode(rndGen)];
std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
ReachableIteratorUnsorted<Node> ri(grid, *startNode);
const float maxDist = params.distance_m * 1.25 + gridSize_m;
auto skip = [&] (const Node& n) {
const float dist_m = n.getDistanceInMeter(gpStart);
return dist_m > maxDist;
struct RICond {
const GridPoint gpStart;
const float maxDist;
RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
bool visit (const Node& n) const {
const float dist_m = n.getDistanceInMeter(gpStart);
return dist_m < maxDist;
}
};
RICond riCond(gpStart, maxDist);
//for (const Node* dstNode : reachableNodes) {
while(ri.hasNext()) {
// iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);
const Node* dstNode = &ri.next(skip);
// const float dist_m = dstNode->getDistanceInMeter(gpStart);
int numVisitedNodes = 0;
// if (dist_m > maxDist) {
// break;
#define MODE 2
#if (MODE == 1)
double bestNodeP = 0;
const Node* bestNode = nullptr;
ReachableByDepthUnsorted<Node> reach(grid);
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
for (const Node* dstNode : nodes) {
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
double p = 1.0;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
p *= p1;
}
if (p > bestNodeP) {
bestNodeP = p;
bestNode = dstNode;
}
}
// while(ri.hasNext()) {
// const Node* dstNode = &ri.next();
// const Point3 nodeCenter = Helper::gpToP3(*dstNode);
// double p = 1.0;
// for (const WalkEvaluator<Node>* eval : evals) {
// const double p1 = eval->getProbability(start, nodeCenter, params);
// p *= p1;
// }
// if (p > bestNodeP) {
// bestNodeP = p;
// bestNode = dstNode;
// }
// }
for (int i = 0; i < 25; ++i) {
for (int i = 0; i < 10; ++i) {
const Point3 nodeCenter = Helper::gpToP3(*bestNode);
// random position within destination-node
const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);
const float ox = dFinal(rndGen);
const float oy = dFinal(rndGen);
// destination = node-center + offset (within the node's bbox)
const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;
// sanity check
if (start == end) {continue;}
if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
std::cout << "random destination is not part of the grid" << std::endl;
continue;
}
//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
const float walkDist_m = end.getDistance(start);//*1.05;
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, end, params);
const double p1 = eval->getProbability(start, end, walkDist_m, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
//drawer.add(end, p);
}
}
#elif (MODE == 2)
//const Point3 end = drawer.get();
ReachableByDepthUnsorted<Node> reach(grid);
std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);
// all reachable nodes
//while(ri.hasNext()) {
//const Node* dstNode = &ri.next();
for (const Node* dstNode : nodes) {
++numVisitedNodes;
const Point3 nodeCenter = Helper::gpToP3(*dstNode);
// try multiple locations within each reachable node
for (int i = 0; i < 3; ++i) {
// random position within destination-node
const float ox = dFinal(rndGen);
const float oy = dFinal(rndGen);
// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
// sanity check
if (start == end) {continue;}
// if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
// std::cout << "random destination is not part of the grid" << std::endl;
// continue;
// }
//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(pWalk);
p *= p1;
}
if (p > bestP) {bestP = p; best = end;}
// drawer.add(end, p);
}
}
#elif (MODE == 3)
using Reachable = ReachableByDepthWithDistanceSorted<Node>;
using ReachableNode = typename Reachable::Entry;
Reachable reach(grid);
std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);
using Sampler = ReachableSamplerByDepth<Node>;
using SamplerResult = typename Sampler::SampleResult;
Sampler sampler(grid, reachableNodes);
for (int i = 0; i < 1500; ++i) {
const SamplerResult sample = sampler.sample();
double p = 1;
for (const WalkEvaluator<Node>* eval : evals) {
const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params);
p *= p1;
}
if (p > bestP) {bestP = p; best = sample.pos;}
}
#endif
//std::cout << numVisitedNodes << std::endl;
//double drawProb = 0; const Point3 end = drawer.get(drawProb);
const Point3 end = best;
WalkResult res;
if (start == end) {
res.probability = 0;
} else {
res.heading = Heading(start.xy(), end.xy());
res.probability = bestP;
//res.probability = drawProb; // when using DrawList
res.probability = bestP; // when using bestP
}
res.position = end;
return res;

511
lib/Recast/Recast.cpp Normal file
View File

@@ -0,0 +1,511 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <new>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
float rcSqrt(float x)
{
return sqrtf(x);
}
/// @class rcContext
/// @par
///
/// This class does not provide logging or timer functionality on its
/// own. Both must be provided by a concrete implementation
/// by overriding the protected member functions. Also, this class does not
/// provide an interface for extracting log messages. (Only adding them.)
/// So concrete implementations must provide one.
///
/// If no logging or timers are required, just pass an instance of this
/// class through the Recast build process.
///
/// @par
///
/// Example:
/// @code
/// // Where ctx is an instance of rcContext and filepath is a char array.
/// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath);
/// @endcode
void rcContext::log(const rcLogCategory category, const char* format, ...)
{
if (!m_logEnabled)
return;
static const int MSG_SIZE = 512;
char msg[MSG_SIZE];
va_list ap;
va_start(ap, format);
int len = vsnprintf(msg, MSG_SIZE, format, ap);
if (len >= MSG_SIZE)
{
len = MSG_SIZE-1;
msg[MSG_SIZE-1] = '\0';
}
va_end(ap);
doLog(category, msg, len);
}
rcHeightfield* rcAllocHeightfield()
{
return new (rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM)) rcHeightfield;
}
rcHeightfield::rcHeightfield()
: width()
, height()
, bmin()
, bmax()
, cs()
, ch()
, spans()
, pools()
, freelist()
{
}
rcHeightfield::~rcHeightfield()
{
// Delete span array.
rcFree(spans);
// Delete span pools.
while (pools)
{
rcSpanPool* next = pools->next;
rcFree(pools);
pools = next;
}
}
void rcFreeHeightField(rcHeightfield* hf)
{
if (!hf) return;
hf->~rcHeightfield();
rcFree(hf);
}
rcCompactHeightfield* rcAllocCompactHeightfield()
{
rcCompactHeightfield* chf = (rcCompactHeightfield*)rcAlloc(sizeof(rcCompactHeightfield), RC_ALLOC_PERM);
memset(chf, 0, sizeof(rcCompactHeightfield));
return chf;
}
void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
{
if (!chf) return;
rcFree(chf->cells);
rcFree(chf->spans);
rcFree(chf->dist);
rcFree(chf->areas);
rcFree(chf);
}
rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
{
rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM);
memset(lset, 0, sizeof(rcHeightfieldLayerSet));
return lset;
}
void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
{
if (!lset) return;
for (int i = 0; i < lset->nlayers; ++i)
{
rcFree(lset->layers[i].heights);
rcFree(lset->layers[i].areas);
rcFree(lset->layers[i].cons);
}
rcFree(lset->layers);
rcFree(lset);
}
rcContourSet* rcAllocContourSet()
{
rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM);
memset(cset, 0, sizeof(rcContourSet));
return cset;
}
void rcFreeContourSet(rcContourSet* cset)
{
if (!cset) return;
for (int i = 0; i < cset->nconts; ++i)
{
rcFree(cset->conts[i].verts);
rcFree(cset->conts[i].rverts);
}
rcFree(cset->conts);
rcFree(cset);
}
rcPolyMesh* rcAllocPolyMesh()
{
rcPolyMesh* pmesh = (rcPolyMesh*)rcAlloc(sizeof(rcPolyMesh), RC_ALLOC_PERM);
memset(pmesh, 0, sizeof(rcPolyMesh));
return pmesh;
}
void rcFreePolyMesh(rcPolyMesh* pmesh)
{
if (!pmesh) return;
rcFree(pmesh->verts);
rcFree(pmesh->polys);
rcFree(pmesh->regs);
rcFree(pmesh->flags);
rcFree(pmesh->areas);
rcFree(pmesh);
}
rcPolyMeshDetail* rcAllocPolyMeshDetail()
{
rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM);
memset(dmesh, 0, sizeof(rcPolyMeshDetail));
return dmesh;
}
void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh)
{
if (!dmesh) return;
rcFree(dmesh->meshes);
rcFree(dmesh->verts);
rcFree(dmesh->tris);
rcFree(dmesh);
}
void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
{
// Calculate bounding box.
rcVcopy(bmin, verts);
rcVcopy(bmax, verts);
for (int i = 1; i < nv; ++i)
{
const float* v = &verts[i*3];
rcVmin(bmin, v);
rcVmax(bmax, v);
}
}
void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
{
*w = (int)((bmax[0] - bmin[0])/cs+0.5f);
*h = (int)((bmax[2] - bmin[2])/cs+0.5f);
}
/// @par
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcAllocHeightfield, rcHeightfield
bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height,
const float* bmin, const float* bmax,
float cs, float ch)
{
rcIgnoreUnused(ctx);
hf.width = width;
hf.height = height;
rcVcopy(hf.bmin, bmin);
rcVcopy(hf.bmax, bmax);
hf.cs = cs;
hf.ch = ch;
hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM);
if (!hf.spans)
return false;
memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
return true;
}
static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
{
float e0[3], e1[3];
rcVsub(e0, v1, v0);
rcVsub(e1, v2, v0);
rcVcross(norm, e0, e1);
rcVnormalize(norm);
}
/// @par
///
/// Only sets the area id's for the walkable triangles. Does not alter the
/// area id's for unwalkable triangles.
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
const float* verts, int nv,
const int* tris, int nt,
unsigned char* areas)
{
rcIgnoreUnused(ctx);
rcIgnoreUnused(nv);
const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
float norm[3];
for (int i = 0; i < nt; ++i)
{
const int* tri = &tris[i*3];
int a = tri[0];
int b = tri[1];
int c = tri[2];
float aa = verts[6];
float bb = verts[7];
float cc = verts[8];
calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
// Check if the face is walkable.
if (norm[1] > walkableThr)
areas[i] = RC_WALKABLE_AREA;
}
}
/// @par
///
/// Only sets the area id's for the unwalkable triangles. Does not alter the
/// area id's for walkable triangles.
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle,
const float* verts, int /*nv*/,
const int* tris, int nt,
unsigned char* areas)
{
rcIgnoreUnused(ctx);
const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
float norm[3];
for (int i = 0; i < nt; ++i)
{
const int* tri = &tris[i*3];
calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
// Check if the face is walkable.
if (norm[1] <= walkableThr)
areas[i] = RC_NULL_AREA;
}
}
int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf)
{
rcIgnoreUnused(ctx);
const int w = hf.width;
const int h = hf.height;
int spanCount = 0;
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
{
if (s->area != RC_NULL_AREA)
spanCount++;
}
}
}
return spanCount;
}
/// @par
///
/// This is just the beginning of the process of fully building a compact heightfield.
/// Various filters may be applied, then the distance field and regions built.
/// E.g: #rcBuildDistanceField and #rcBuildRegions
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig
bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
rcHeightfield& hf, rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
const int w = hf.width;
const int h = hf.height;
const int spanCount = rcGetHeightFieldSpanCount(ctx, hf);
// Fill in header.
chf.width = w;
chf.height = h;
chf.spanCount = spanCount;
chf.walkableHeight = walkableHeight;
chf.walkableClimb = walkableClimb;
chf.maxRegions = 0;
rcVcopy(chf.bmin, hf.bmin);
rcVcopy(chf.bmax, hf.bmax);
chf.bmax[1] += walkableHeight*hf.ch;
chf.cs = hf.cs;
chf.ch = hf.ch;
chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM);
if (!chf.cells)
{
ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
return false;
}
memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM);
if (!chf.spans)
{
ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
return false;
}
memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM);
if (!chf.areas)
{
ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
return false;
}
memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*spanCount);
const int MAX_HEIGHT = 0xffff;
// Fill in cells and spans.
int idx = 0;
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcSpan* s = hf.spans[x + y*w];
// If there are no spans at this cell, just leave the data to index=0, count=0.
if (!s) continue;
rcCompactCell& c = chf.cells[x+y*w];
c.index = idx;
c.count = 0;
while (s)
{
if (s->area != RC_NULL_AREA)
{
const int bot = (int)s->smax;
const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
chf.areas[idx] = s->area;
idx++;
c.count++;
}
s = s->next;
}
}
}
// Find neighbour connections.
const int MAX_LAYERS = RC_NOT_CONNECTED-1;
int tooHighNeighbour = 0;
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
for (int dir = 0; dir < 4; ++dir)
{
rcSetCon(s, dir, RC_NOT_CONNECTED);
const int nx = x + rcGetDirOffsetX(dir);
const int ny = y + rcGetDirOffsetY(dir);
// First check that the neighbour cell is in bounds.
if (nx < 0 || ny < 0 || nx >= w || ny >= h)
continue;
// Iterate over all neighbour spans and check if any of the is
// accessible from current cell.
const rcCompactCell& nc = chf.cells[nx+ny*w];
for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
{
const rcCompactSpan& ns = chf.spans[k];
const int bot = rcMax(s.y, ns.y);
const int top = rcMin(s.y+s.h, ns.y+ns.h);
// Check that the gap between the spans is walkable,
// and that the climb height between the gaps is not too high.
if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
{
// Mark direction as walkable.
const int lidx = k - (int)nc.index;
if (lidx < 0 || lidx > MAX_LAYERS)
{
tooHighNeighbour = rcMax(tooHighNeighbour, lidx);
continue;
}
rcSetCon(s, dir, lidx);
break;
}
}
}
}
}
}
if (tooHighNeighbour > MAX_LAYERS)
{
ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
tooHighNeighbour, MAX_LAYERS);
}
return true;
}
/*
static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
{
int size = 0;
size += sizeof(hf);
size += hf.width * hf.height * sizeof(rcSpan*);
rcSpanPool* pool = hf.pools;
while (pool)
{
size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
pool = pool->next;
}
return size;
}
static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
{
int size = 0;
size += sizeof(rcCompactHeightfield);
size += sizeof(rcCompactSpan) * chf.spanCount;
size += sizeof(rcCompactCell) * chf.width * chf.height;
return size;
}
*/

1206
lib/Recast/Recast.h Normal file
View File

File diff suppressed because it is too large Load Diff

86
lib/Recast/RecastAlloc.cpp Normal file
View File

@@ -0,0 +1,86 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <stdlib.h>
#include <string.h>
#include "RecastAlloc.h"
#include "RecastAssert.h"
static void *rcAllocDefault(size_t size, rcAllocHint)
{
return malloc(size);
}
static void rcFreeDefault(void *ptr)
{
free(ptr);
}
static rcAllocFunc* sRecastAllocFunc = rcAllocDefault;
static rcFreeFunc* sRecastFreeFunc = rcFreeDefault;
/// @see rcAlloc, rcFree
void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc)
{
sRecastAllocFunc = allocFunc ? allocFunc : rcAllocDefault;
sRecastFreeFunc = freeFunc ? freeFunc : rcFreeDefault;
}
/// @see rcAllocSetCustom
void* rcAlloc(size_t size, rcAllocHint hint)
{
return sRecastAllocFunc(size, hint);
}
/// @par
///
/// @warning This function leaves the value of @p ptr unchanged. So it still
/// points to the same (now invalid) location, and not to null.
///
/// @see rcAllocSetCustom
void rcFree(void* ptr)
{
if (ptr)
sRecastFreeFunc(ptr);
}
/// @class rcIntArray
///
/// While it is possible to pre-allocate a specific array size during
/// construction or by using the #resize method, certain methods will
/// automatically resize the array as needed.
///
/// @warning The array memory is not initialized to zero when the size is
/// manually set during construction or when using #resize.
/// @par
///
/// Using this method ensures the array is at least large enough to hold
/// the specified number of elements. This can improve performance by
/// avoiding auto-resizing during use.
void rcIntArray::doResize(int n)
{
if (!m_cap) m_cap = n;
while (m_cap < n) m_cap *= 2;
int* newData = (int*)rcAlloc(m_cap*sizeof(int), RC_ALLOC_TEMP);
rcAssert(newData);
if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int));
rcFree(m_data);
m_data = newData;
}

146
lib/Recast/RecastAlloc.h Normal file
View File

@@ -0,0 +1,146 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#ifndef RECASTALLOC_H
#define RECASTALLOC_H
#include <stddef.h>
/// Provides hint values to the memory allocator on how long the
/// memory is expected to be used.
enum rcAllocHint
{
RC_ALLOC_PERM, ///< Memory will persist after a function call.
RC_ALLOC_TEMP ///< Memory used temporarily within a function.
};
/// A memory allocation function.
// @param[in] size The size, in bytes of memory, to allocate.
// @param[in] rcAllocHint A hint to the allocator on how long the memory is expected to be in use.
// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
/// @see rcAllocSetCustom
typedef void* (rcAllocFunc)(size_t size, rcAllocHint hint);
/// A memory deallocation function.
/// @param[in] ptr A pointer to a memory block previously allocated using #rcAllocFunc.
/// @see rcAllocSetCustom
typedef void (rcFreeFunc)(void* ptr);
/// Sets the base custom allocation functions to be used by Recast.
/// @param[in] allocFunc The memory allocation function to be used by #rcAlloc
/// @param[in] freeFunc The memory de-allocation function to be used by #rcFree
void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc);
/// Allocates a memory block.
/// @param[in] size The size, in bytes of memory, to allocate.
/// @param[in] hint A hint to the allocator on how long the memory is expected to be in use.
/// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
/// @see rcFree
void* rcAlloc(size_t size, rcAllocHint hint);
/// Deallocates a memory block.
/// @param[in] ptr A pointer to a memory block previously allocated using #rcAlloc.
/// @see rcAlloc
void rcFree(void* ptr);
/// A simple dynamic array of integers.
class rcIntArray
{
int* m_data;
int m_size, m_cap;
void doResize(int n);
// Explicitly disabled copy constructor and copy assignment operator.
rcIntArray(const rcIntArray&);
rcIntArray& operator=(const rcIntArray&);
public:
/// Constructs an instance with an initial array size of zero.
rcIntArray() : m_data(0), m_size(0), m_cap(0) {}
/// Constructs an instance initialized to the specified size.
/// @param[in] n The initial size of the integer array.
rcIntArray(int n) : m_data(0), m_size(0), m_cap(0) { resize(n); }
~rcIntArray() { rcFree(m_data); }
/// Specifies the new size of the integer array.
/// @param[in] n The new size of the integer array.
void resize(int n)
{
if (n > m_cap)
doResize(n);
m_size = n;
}
/// Push the specified integer onto the end of the array and increases the size by one.
/// @param[in] item The new value.
void push(int item) { resize(m_size+1); m_data[m_size-1] = item; }
/// Returns the value at the end of the array and reduces the size by one.
/// @return The value at the end of the array.
int pop()
{
if (m_size > 0)
m_size--;
return m_data[m_size];
}
/// The value at the specified array index.
/// @warning Does not provide overflow protection.
/// @param[in] i The index of the value.
const int& operator[](int i) const { return m_data[i]; }
/// The value at the specified array index.
/// @warning Does not provide overflow protection.
/// @param[in] i The index of the value.
int& operator[](int i) { return m_data[i]; }
/// The current size of the integer array.
int size() const { return m_size; }
};
/// A simple helper class used to delete an array when it goes out of scope.
/// @note This class is rarely if ever used by the end user.
template<class T> class rcScopedDelete
{
T* ptr;
public:
/// Constructs an instance with a null pointer.
inline rcScopedDelete() : ptr(0) {}
/// Constructs an instance with the specified pointer.
/// @param[in] p An pointer to an allocated array.
inline rcScopedDelete(T* p) : ptr(p) {}
inline ~rcScopedDelete() { rcFree(ptr); }
/// The root array pointer.
/// @return The root array pointer.
inline operator T*() { return ptr; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
rcScopedDelete(const rcScopedDelete&);
rcScopedDelete& operator=(const rcScopedDelete&);
};
#endif

591
lib/Recast/RecastArea.cpp Normal file
View File

@@ -0,0 +1,591 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
/// @par
///
/// Basically, any spans that are closer to a boundary or obstruction than the specified radius
/// are marked as unwalkable.
///
/// This method is usually called immediately after the heightfield has been built.
///
/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius
bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
{
rcAssert(ctx);
const int w = chf.width;
const int h = chf.height;
rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA);
unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
if (!dist)
{
ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount);
return false;
}
// Init distance.
memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount);
// Mark boundary cells.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
if (chf.areas[i] == RC_NULL_AREA)
{
dist[i] = 0;
}
else
{
const rcCompactSpan& s = chf.spans[i];
int nc = 0;
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int nx = x + rcGetDirOffsetX(dir);
const int ny = y + rcGetDirOffsetY(dir);
const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
if (chf.areas[nidx] != RC_NULL_AREA)
{
nc++;
}
}
}
// At least one missing neighbour.
if (nc != 4)
dist[i] = 0;
}
}
}
}
unsigned char nd;
// Pass 1
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
{
// (-1,0)
const int ax = x + rcGetDirOffsetX(0);
const int ay = y + rcGetDirOffsetY(0);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (-1,-1)
if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(3);
const int aay = ay + rcGetDirOffsetY(3);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
{
// (0,-1)
const int ax = x + rcGetDirOffsetX(3);
const int ay = y + rcGetDirOffsetY(3);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (1,-1)
if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(2);
const int aay = ay + rcGetDirOffsetY(2);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
}
}
}
// Pass 2
for (int y = h-1; y >= 0; --y)
{
for (int x = w-1; x >= 0; --x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
{
// (1,0)
const int ax = x + rcGetDirOffsetX(2);
const int ay = y + rcGetDirOffsetY(2);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (1,1)
if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(1);
const int aay = ay + rcGetDirOffsetY(1);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
{
// (0,1)
const int ax = x + rcGetDirOffsetX(1);
const int ay = y + rcGetDirOffsetY(1);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (-1,1)
if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(0);
const int aay = ay + rcGetDirOffsetY(0);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
}
}
}
const unsigned char thr = (unsigned char)(radius*2);
for (int i = 0; i < chf.spanCount; ++i)
if (dist[i] < thr)
chf.areas[i] = RC_NULL_AREA;
rcFree(dist);
return true;
}
static void insertSort(unsigned char* a, const int n)
{
int i, j;
for (i = 1; i < n; i++)
{
const unsigned char value = a[i];
for (j = i - 1; j >= 0 && a[j] > value; j--)
a[j+1] = a[j];
a[j+1] = value;
}
}
/// @par
///
/// This filter is usually applied after applying area id's using functions
/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea.
///
/// @see rcCompactHeightfield
bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf)
{
rcAssert(ctx);
const int w = chf.width;
const int h = chf.height;
rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA);
unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
if (!areas)
{
ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount);
return false;
}
// Init distance.
memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount);
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
{
areas[i] = chf.areas[i];
continue;
}
unsigned char nei[9];
for (int j = 0; j < 9; ++j)
nei[j] = chf.areas[i];
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
if (chf.areas[ai] != RC_NULL_AREA)
nei[dir*2+0] = chf.areas[ai];
const rcCompactSpan& as = chf.spans[ai];
const int dir2 = (dir+1) & 0x3;
if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
{
const int ax2 = ax + rcGetDirOffsetX(dir2);
const int ay2 = ay + rcGetDirOffsetY(dir2);
const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
if (chf.areas[ai2] != RC_NULL_AREA)
nei[dir*2+1] = chf.areas[ai2];
}
}
}
insertSort(nei, 9);
areas[i] = nei[4];
}
}
}
memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount);
rcFree(areas);
return true;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA);
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if ((int)s.y >= miny && (int)s.y <= maxy)
{
if (chf.areas[i] != RC_NULL_AREA)
chf.areas[i] = areaId;
}
}
}
}
}
static int pointInPoly(int nvert, const float* verts, const float* p)
{
int i, j, c = 0;
for (i = 0, j = nvert-1; i < nvert; j = i++)
{
const float* vi = &verts[i*3];
const float* vj = &verts[j*3];
if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
(p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
c = !c;
}
return c;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// The y-values of the polygon vertices are ignored. So the polygon is effectively
/// projected onto the xz-plane at @p hmin, then extruded to @p hmax.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
const float hmin, const float hmax, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA);
float bmin[3], bmax[3];
rcVcopy(bmin, verts);
rcVcopy(bmax, verts);
for (int i = 1; i < nverts; ++i)
{
rcVmin(bmin, &verts[i*3]);
rcVmax(bmax, &verts[i*3]);
}
bmin[1] = hmin;
bmax[1] = hmax;
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
// TODO: Optimize.
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
continue;
if ((int)s.y >= miny && (int)s.y <= maxy)
{
float p[3];
p[0] = chf.bmin[0] + (x+0.5f)*chf.cs;
p[1] = 0;
p[2] = chf.bmin[2] + (z+0.5f)*chf.cs;
if (pointInPoly(nverts, verts, p))
{
chf.areas[i] = areaId;
}
}
}
}
}
}
int rcOffsetPoly(const float* verts, const int nverts, const float offset,
float* outVerts, const int maxOutVerts)
{
const float MITER_LIMIT = 1.20f;
int n = 0;
for (int i = 0; i < nverts; i++)
{
const int a = (i+nverts-1) % nverts;
const int b = i;
const int c = (i+1) % nverts;
const float* va = &verts[a*3];
const float* vb = &verts[b*3];
const float* vc = &verts[c*3];
float dx0 = vb[0] - va[0];
float dy0 = vb[2] - va[2];
float d0 = dx0*dx0 + dy0*dy0;
if (d0 > 1e-6f)
{
d0 = 1.0f/rcSqrt(d0);
dx0 *= d0;
dy0 *= d0;
}
float dx1 = vc[0] - vb[0];
float dy1 = vc[2] - vb[2];
float d1 = dx1*dx1 + dy1*dy1;
if (d1 > 1e-6f)
{
d1 = 1.0f/rcSqrt(d1);
dx1 *= d1;
dy1 *= d1;
}
const float dlx0 = -dy0;
const float dly0 = dx0;
const float dlx1 = -dy1;
const float dly1 = dx1;
float cross = dx1*dy0 - dx0*dy1;
float dmx = (dlx0 + dlx1) * 0.5f;
float dmy = (dly0 + dly1) * 0.5f;
float dmr2 = dmx*dmx + dmy*dmy;
bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f;
if (dmr2 > 1e-6f)
{
const float scale = 1.0f / dmr2;
dmx *= scale;
dmy *= scale;
}
if (bevel && cross < 0.0f)
{
if (n+2 >= maxOutVerts)
return 0;
float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f;
outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset;
n++;
outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset;
n++;
}
else
{
if (n+1 >= maxOutVerts)
return 0;
outVerts[n*3+0] = vb[0] - dmx*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] - dmy*offset;
n++;
}
}
return n;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkCylinderArea(rcContext* ctx, const float* pos,
const float r, const float h, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA);
float bmin[3], bmax[3];
bmin[0] = pos[0] - r;
bmin[1] = pos[1];
bmin[2] = pos[2] - r;
bmax[0] = pos[0] + r;
bmax[1] = pos[1] + h;
bmax[2] = pos[2] + r;
const float r2 = r*r;
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
continue;
if ((int)s.y >= miny && (int)s.y <= maxy)
{
const float sx = chf.bmin[0] + (x+0.5f)*chf.cs;
const float sz = chf.bmin[2] + (z+0.5f)*chf.cs;
const float dx = sx - pos[0];
const float dz = sz - pos[2];
if (dx*dx + dz*dz < r2)
{
chf.areas[i] = areaId;
}
}
}
}
}
}

35
lib/Recast/RecastAssert.cpp Normal file
View File

@@ -0,0 +1,35 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "RecastAssert.h"
#ifndef NDEBUG
static rcAssertFailFunc* sRecastAssertFailFunc = 0;
void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc)
{
sRecastAssertFailFunc = assertFailFunc;
}
rcAssertFailFunc* rcAssertFailGetCustom()
{
return sRecastAssertFailFunc;
}
#endif

56
lib/Recast/RecastAssert.h Normal file
View File

@@ -0,0 +1,56 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#ifndef RECASTASSERT_H
#define RECASTASSERT_H
// Note: This header file's only purpose is to include define assert.
// Feel free to change the file and include your own implementation instead.
#ifdef NDEBUG
// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
# define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false)
#else
/// An assertion failure function.
// @param[in] expression asserted expression.
// @param[in] file Filename of the failed assertion.
// @param[in] line Line number of the failed assertion.
/// @see rcAssertFailSetCustom
typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line);
/// Sets the base custom assertion failure function to be used by Recast.
/// @param[in] assertFailFunc The function to be used in case of failure of #dtAssert
void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc);
/// Gets the base custom assertion failure function to be used by Recast.
rcAssertFailFunc* rcAssertFailGetCustom();
# include <assert.h>
# define rcAssert(expression) \
{ \
rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \
if(failFunc == NULL) { assert(expression); } \
else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
}
#endif
#endif // RECASTASSERT_H

1105
lib/Recast/RecastContour.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

202
lib/Recast/RecastFilter.cpp Normal file
View File

@@ -0,0 +1,202 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAssert.h"
/// @par
///
/// Allows the formation of walkable regions that will flow over low lying
/// objects such as curbs, and up structures such as stairways.
///
/// Two neighboring spans are walkable if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb</tt>
///
/// @warning Will override the effect of #rcFilterLedgeSpans. So if both filters are used, call
/// #rcFilterLedgeSpans after calling this filter.
///
/// @see rcHeightfield, rcConfig
void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_FILTER_LOW_OBSTACLES);
const int w = solid.width;
const int h = solid.height;
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
rcSpan* ps = 0;
bool previousWalkable = false;
unsigned char previousArea = RC_NULL_AREA;
for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next)
{
const bool walkable = s->area != RC_NULL_AREA;
// If current span is not walkable, but there is walkable
// span just below it, mark the span above it walkable too.
if (!walkable && previousWalkable)
{
if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb)
s->area = previousArea;
}
// Copy walkable flag so that it cannot propagate
// past multiple non-walkable objects.
previousWalkable = walkable;
previousArea = s->area;
}
}
}
}
/// @par
///
/// A ledge is a span with one or more neighbors whose maximum is further away than @p walkableClimb
/// from the current span's maximum.
/// This method removes the impact of the overestimation of conservative voxelization
/// so the resulting mesh will not have regions hanging in the air over ledges.
///
/// A span is a ledge if: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) > walkableClimb</tt>
///
/// @see rcHeightfield, rcConfig
void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight, const int walkableClimb,
rcHeightfield& solid)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_FILTER_BORDER);
const int w = solid.width;
const int h = solid.height;
const int MAX_HEIGHT = 0xffff;
// Mark border spans.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
{
// Skip non walkable spans.
if (s->area == RC_NULL_AREA)
continue;
const int bot = (int)(s->smax);
const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
// Find neighbours minimum height.
int minh = MAX_HEIGHT;
// Min and max height of accessible neighbours.
int asmin = s->smax;
int asmax = s->smax;
for (int dir = 0; dir < 4; ++dir)
{
int dx = x + rcGetDirOffsetX(dir);
int dy = y + rcGetDirOffsetY(dir);
// Skip neighbours which are out of bounds.
if (dx < 0 || dy < 0 || dx >= w || dy >= h)
{
minh = rcMin(minh, -walkableClimb - bot);
continue;
}
// From minus infinity to the first span.
rcSpan* ns = solid.spans[dx + dy*w];
int nbot = -walkableClimb;
int ntop = ns ? (int)ns->smin : MAX_HEIGHT;
// Skip neightbour if the gap between the spans is too small.
if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
minh = rcMin(minh, nbot - bot);
// Rest of the spans.
for (ns = solid.spans[dx + dy*w]; ns; ns = ns->next)
{
nbot = (int)ns->smax;
ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT;
// Skip neightbour if the gap between the spans is too small.
if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight)
{
minh = rcMin(minh, nbot - bot);
// Find min/max accessible neighbour height.
if (rcAbs(nbot - bot) <= walkableClimb)
{
if (nbot < asmin) asmin = nbot;
if (nbot > asmax) asmax = nbot;
}
}
}
}
// The current span is close to a ledge if the drop to any
// neighbour span is less than the walkableClimb.
if (minh < -walkableClimb)
{
s->area = RC_NULL_AREA;
}
// If the difference between all neighbours is too large,
// we are at steep slope, mark the span as ledge.
else if ((asmax - asmin) > walkableClimb)
{
s->area = RC_NULL_AREA;
}
}
}
}
}
/// @par
///
/// For this filter, the clearance above the span is the distance from the span's
/// maximum to the next higher span's minimum. (Same grid column.)
///
/// @see rcHeightfield, rcConfig
void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_FILTER_WALKABLE);
const int w = solid.width;
const int h = solid.height;
const int MAX_HEIGHT = 0xffff;
// Remove walkable flag from spans which do not have enough
// space above them for the agent to stand there.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next)
{
const int bot = (int)(s->smax);
const int top = s->next ? (int)(s->next->smin) : MAX_HEIGHT;
if ((top - bot) <= walkableHeight)
s->area = RC_NULL_AREA;
}
}
}
}

644
lib/Recast/RecastLayers.cpp Normal file
View File

@@ -0,0 +1,644 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
// Must be 255 or smaller (not 256) because layer IDs are stored as
// a byte where 255 is a special value.
static const int RC_MAX_LAYERS = 63;
static const int RC_MAX_NEIS = 16;
struct rcLayerRegion
{
unsigned char layers[RC_MAX_LAYERS];
unsigned char neis[RC_MAX_NEIS];
unsigned short ymin, ymax;
unsigned char layerId; // Layer ID
unsigned char nlayers; // Layer count
unsigned char nneis; // Neighbour count
unsigned char base; // Flag indicating if the region is the base of merged regions.
};
static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v)
{
const int n = (int)an;
for (int i = 0; i < n; ++i)
{
if (a[i] == v)
return true;
}
return false;
}
static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v)
{
if (contains(a, an, v))
return true;
if ((int)an >= anMax)
return false;
a[an] = v;
an++;
return true;
}
inline bool overlapRange(const unsigned short amin, const unsigned short amax,
const unsigned short bmin, const unsigned short bmax)
{
return (amin > bmax || amax < bmin) ? false : true;
}
struct rcLayerSweepSpan
{
unsigned short ns; // number samples
unsigned char id; // region id
unsigned char nei; // neighbour id
};
/// @par
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig
bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf,
const int borderSize, const int walkableHeight,
rcHeightfieldLayerSet& lset)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_BUILD_LAYERS);
const int w = chf.width;
const int h = chf.height;
rcScopedDelete<unsigned char> srcReg((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP));
if (!srcReg)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount);
return false;
}
memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount);
const int nsweeps = chf.width;
rcScopedDelete<rcLayerSweepSpan> sweeps((rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP));
if (!sweeps)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps);
return false;
}
// Partition walkable area into monotone regions.
int prevCount[256];
unsigned char regId = 0;
for (int y = borderSize; y < h-borderSize; ++y)
{
memset(prevCount,0,sizeof(int)*regId);
unsigned char sweepId = 0;
for (int x = borderSize; x < w-borderSize; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA) continue;
unsigned char sid = 0xff;
// -x
if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(0);
const int ay = y + rcGetDirOffsetY(0);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff)
sid = srcReg[ai];
}
if (sid == 0xff)
{
sid = sweepId++;
sweeps[sid].nei = 0xff;
sweeps[sid].ns = 0;
}
// -y
if (rcGetCon(s,3) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(3);
const int ay = y + rcGetDirOffsetY(3);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
const unsigned char nr = srcReg[ai];
if (nr != 0xff)
{
// Set neighbour when first valid neighbour is encoutered.
if (sweeps[sid].ns == 0)
sweeps[sid].nei = nr;
if (sweeps[sid].nei == nr)
{
// Update existing neighbour
sweeps[sid].ns++;
prevCount[nr]++;
}
else
{
// This is hit if there is nore than one neighbour.
// Invalidate the neighbour.
sweeps[sid].nei = 0xff;
}
}
}
srcReg[i] = sid;
}
}
// Create unique ID.
for (int i = 0; i < sweepId; ++i)
{
// If the neighbour is set and there is only one continuous connection to it,
// the sweep will be merged with the previous one, else new region is created.
if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns)
{
sweeps[i].id = sweeps[i].nei;
}
else
{
if (regId == 255)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow.");
return false;
}
sweeps[i].id = regId++;
}
}
// Remap local sweep ids to region ids.
for (int x = borderSize; x < w-borderSize; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
if (srcReg[i] != 0xff)
srcReg[i] = sweeps[srcReg[i]].id;
}
}
}
// Allocate and init layer regions.
const int nregs = (int)regId;
rcScopedDelete<rcLayerRegion> regs((rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP));
if (!regs)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs);
return false;
}
memset(regs, 0, sizeof(rcLayerRegion)*nregs);
for (int i = 0; i < nregs; ++i)
{
regs[i].layerId = 0xff;
regs[i].ymin = 0xffff;
regs[i].ymax = 0;
}
// Find region neighbours and overlapping regions.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
unsigned char lregs[RC_MAX_LAYERS];
int nlregs = 0;
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
const unsigned char ri = srcReg[i];
if (ri == 0xff) continue;
regs[ri].ymin = rcMin(regs[ri].ymin, s.y);
regs[ri].ymax = rcMax(regs[ri].ymax, s.y);
// Collect all region layers.
if (nlregs < RC_MAX_LAYERS)
lregs[nlregs++] = ri;
// Update neighbours
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
const unsigned char rai = srcReg[ai];
if (rai != 0xff && rai != ri)
{
// Don't check return value -- if we cannot add the neighbor
// it will just cause a few more regions to be created, which
// is fine.
addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai);
}
}
}
}
// Update overlapping regions.
for (int i = 0; i < nlregs-1; ++i)
{
for (int j = i+1; j < nlregs; ++j)
{
if (lregs[i] != lregs[j])
{
rcLayerRegion& ri = regs[lregs[i]];
rcLayerRegion& rj = regs[lregs[j]];
if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) ||
!addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
}
}
}
}
// Create 2D layers from regions.
unsigned char layerId = 0;
static const int MAX_STACK = 64;
unsigned char stack[MAX_STACK];
int nstack = 0;
for (int i = 0; i < nregs; ++i)
{
rcLayerRegion& root = regs[i];
// Skip already visited.
if (root.layerId != 0xff)
continue;
// Start search.
root.layerId = layerId;
root.base = 1;
nstack = 0;
stack[nstack++] = (unsigned char)i;
while (nstack)
{
// Pop front
rcLayerRegion& reg = regs[stack[0]];
nstack--;
for (int j = 0; j < nstack; ++j)
stack[j] = stack[j+1];
const int nneis = (int)reg.nneis;
for (int j = 0; j < nneis; ++j)
{
const unsigned char nei = reg.neis[j];
rcLayerRegion& regn = regs[nei];
// Skip already visited.
if (regn.layerId != 0xff)
continue;
// Skip if the neighbour is overlapping root region.
if (contains(root.layers, root.nlayers, nei))
continue;
// Skip if the height range would become too large.
const int ymin = rcMin(root.ymin, regn.ymin);
const int ymax = rcMax(root.ymax, regn.ymax);
if ((ymax - ymin) >= 255)
continue;
if (nstack < MAX_STACK)
{
// Deepen
stack[nstack++] = (unsigned char)nei;
// Mark layer id
regn.layerId = layerId;
// Merge current layers to root.
for (int k = 0; k < regn.nlayers; ++k)
{
if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
root.ymin = rcMin(root.ymin, regn.ymin);
root.ymax = rcMax(root.ymax, regn.ymax);
}
}
}
layerId++;
}
// Merge non-overlapping regions that are close in height.
const unsigned short mergeHeight = (unsigned short)walkableHeight * 4;
for (int i = 0; i < nregs; ++i)
{
rcLayerRegion& ri = regs[i];
if (!ri.base) continue;
unsigned char newId = ri.layerId;
for (;;)
{
unsigned char oldId = 0xff;
for (int j = 0; j < nregs; ++j)
{
if (i == j) continue;
rcLayerRegion& rj = regs[j];
if (!rj.base) continue;
// Skip if the regions are not close to each other.
if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight))
continue;
// Skip if the height range would become too large.
const int ymin = rcMin(ri.ymin, rj.ymin);
const int ymax = rcMax(ri.ymax, rj.ymax);
if ((ymax - ymin) >= 255)
continue;
// Make sure that there is no overlap when merging 'ri' and 'rj'.
bool overlap = false;
// Iterate over all regions which have the same layerId as 'rj'
for (int k = 0; k < nregs; ++k)
{
if (regs[k].layerId != rj.layerId)
continue;
// Check if region 'k' is overlapping region 'ri'
// Index to 'regs' is the same as region id.
if (contains(ri.layers,ri.nlayers, (unsigned char)k))
{
overlap = true;
break;
}
}
// Cannot merge of regions overlap.
if (overlap)
continue;
// Can merge i and j.
oldId = rj.layerId;
break;
}
// Could not find anything to merge with, stop.
if (oldId == 0xff)
break;
// Merge
for (int j = 0; j < nregs; ++j)
{
rcLayerRegion& rj = regs[j];
if (rj.layerId == oldId)
{
rj.base = 0;
// Remap layerIds.
rj.layerId = newId;
// Add overlaid layers from 'rj' to 'ri'.
for (int k = 0; k < rj.nlayers; ++k)
{
if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
// Update height bounds.
ri.ymin = rcMin(ri.ymin, rj.ymin);
ri.ymax = rcMax(ri.ymax, rj.ymax);
}
}
}
}
// Compact layerIds
unsigned char remap[256];
memset(remap, 0, 256);
// Find number of unique layers.
layerId = 0;
for (int i = 0; i < nregs; ++i)
remap[regs[i].layerId] = 1;
for (int i = 0; i < 256; ++i)
{
if (remap[i])
remap[i] = layerId++;
else
remap[i] = 0xff;
}
// Remap ids.
for (int i = 0; i < nregs; ++i)
regs[i].layerId = remap[regs[i].layerId];
// No layers, return empty.
if (layerId == 0)
return true;
// Create layers.
rcAssert(lset.layers == 0);
const int lw = w - borderSize*2;
const int lh = h - borderSize*2;
// Build contracted bbox for layers.
float bmin[3], bmax[3];
rcVcopy(bmin, chf.bmin);
rcVcopy(bmax, chf.bmax);
bmin[0] += borderSize*chf.cs;
bmin[2] += borderSize*chf.cs;
bmax[0] -= borderSize*chf.cs;
bmax[2] -= borderSize*chf.cs;
lset.nlayers = (int)layerId;
lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM);
if (!lset.layers)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers);
return false;
}
memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers);
// Store layers.
for (int i = 0; i < lset.nlayers; ++i)
{
unsigned char curId = (unsigned char)i;
rcHeightfieldLayer* layer = &lset.layers[i];
const int gridSize = sizeof(unsigned char)*lw*lh;
layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->heights)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize);
return false;
}
memset(layer->heights, 0xff, gridSize);
layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->areas)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize);
return false;
}
memset(layer->areas, 0, gridSize);
layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->cons)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize);
return false;
}
memset(layer->cons, 0, gridSize);
// Find layer height bounds.
int hmin = 0, hmax = 0;
for (int j = 0; j < nregs; ++j)
{
if (regs[j].base && regs[j].layerId == curId)
{
hmin = (int)regs[j].ymin;
hmax = (int)regs[j].ymax;
}
}
layer->width = lw;
layer->height = lh;
layer->cs = chf.cs;
layer->ch = chf.ch;
// Adjust the bbox to fit the heightfield.
rcVcopy(layer->bmin, bmin);
rcVcopy(layer->bmax, bmax);
layer->bmin[1] = bmin[1] + hmin*chf.ch;
layer->bmax[1] = bmin[1] + hmax*chf.ch;
layer->hmin = hmin;
layer->hmax = hmax;
// Update usable data region.
layer->minx = layer->width;
layer->maxx = 0;
layer->miny = layer->height;
layer->maxy = 0;
// Copy height and area from compact heightfield.
for (int y = 0; y < lh; ++y)
{
for (int x = 0; x < lw; ++x)
{
const int cx = borderSize+x;
const int cy = borderSize+y;
const rcCompactCell& c = chf.cells[cx+cy*w];
for (int j = (int)c.index, nj = (int)(c.index+c.count); j < nj; ++j)
{
const rcCompactSpan& s = chf.spans[j];
// Skip unassigned regions.
if (srcReg[j] == 0xff)
continue;
// Skip of does nto belong to current layer.
unsigned char lid = regs[srcReg[j]].layerId;
if (lid != curId)
continue;
// Update data bounds.
layer->minx = rcMin(layer->minx, x);
layer->maxx = rcMax(layer->maxx, x);
layer->miny = rcMin(layer->miny, y);
layer->maxy = rcMax(layer->maxy, y);
// Store height and area type.
const int idx = x+y*lw;
layer->heights[idx] = (unsigned char)(s.y - hmin);
layer->areas[idx] = chf.areas[j];
// Check connection.
unsigned char portal = 0;
unsigned char con = 0;
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = cx + rcGetDirOffsetX(dir);
const int ay = cy + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff;
// Portal mask
if (chf.areas[ai] != RC_NULL_AREA && lid != alid)
{
portal |= (unsigned char)(1<<dir);
// Update height so that it matches on both sides of the portal.
const rcCompactSpan& as = chf.spans[ai];
if (as.y > hmin)
layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin));
}
// Valid connection mask
if (chf.areas[ai] != RC_NULL_AREA && lid == alid)
{
const int nx = ax - borderSize;
const int ny = ay - borderSize;
if (nx >= 0 && ny >= 0 && nx < lw && ny < lh)
con |= (unsigned char)(1<<dir);
}
}
}
layer->cons[idx] = (portal << 4) | con;
}
}
}
if (layer->minx > layer->maxx)
layer->minx = layer->maxx = 0;
if (layer->miny > layer->maxy)
layer->miny = layer->maxy = 0;
}
return true;
}

1552
lib/Recast/RecastMesh.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1462
lib/Recast/RecastMeshDetail.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

454
lib/Recast/RecastRasterization.cpp Normal file
View File

@@ -0,0 +1,454 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
inline bool overlapBounds(const float* amin, const float* amax, const float* bmin, const float* bmax)
{
bool overlap = true;
overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
return overlap;
}
inline bool overlapInterval(unsigned short amin, unsigned short amax,
unsigned short bmin, unsigned short bmax)
{
if (amax < bmin) return false;
if (amin > bmax) return false;
return true;
}
static rcSpan* allocSpan(rcHeightfield& hf)
{
// If running out of memory, allocate new page and update the freelist.
if (!hf.freelist || !hf.freelist->next)
{
// Create new page.
// Allocate memory for the new pool.
rcSpanPool* pool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM);
if (!pool) return 0;
// Add the pool into the list of pools.
pool->next = hf.pools;
hf.pools = pool;
// Add new items to the free list.
rcSpan* freelist = hf.freelist;
rcSpan* head = &pool->items[0];
rcSpan* it = &pool->items[RC_SPANS_PER_POOL];
do
{
--it;
it->next = freelist;
freelist = it;
}
while (it != head);
hf.freelist = it;
}
// Pop item from in front of the free list.
rcSpan* it = hf.freelist;
hf.freelist = hf.freelist->next;
return it;
}
static void freeSpan(rcHeightfield& hf, rcSpan* ptr)
{
if (!ptr) return;
// Add the node in front of the free list.
ptr->next = hf.freelist;
hf.freelist = ptr;
}
static bool addSpan(rcHeightfield& hf, const int x, const int y,
const unsigned short smin, const unsigned short smax,
const unsigned char area, const int flagMergeThr)
{
int idx = x + y*hf.width;
rcSpan* s = allocSpan(hf);
if (!s)
return false;
s->smin = smin;
s->smax = smax;
s->area = area;
s->next = 0;
// Empty cell, add the first span.
if (!hf.spans[idx])
{
hf.spans[idx] = s;
return true;
}
rcSpan* prev = 0;
rcSpan* cur = hf.spans[idx];
// Insert and merge spans.
while (cur)
{
if (cur->smin > s->smax)
{
// Current span is further than the new span, break.
break;
}
else if (cur->smax < s->smin)
{
// Current span is before the new span advance.
prev = cur;
cur = cur->next;
}
else
{
// Merge spans.
if (cur->smin < s->smin)
s->smin = cur->smin;
if (cur->smax > s->smax)
s->smax = cur->smax;
// Merge flags.
if (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr)
s->area = rcMax(s->area, cur->area);
// Remove current span.
rcSpan* next = cur->next;
freeSpan(hf, cur);
if (prev)
prev->next = next;
else
hf.spans[idx] = next;
cur = next;
}
}
// Insert new span.
if (prev)
{
s->next = prev->next;
prev->next = s;
}
else
{
s->next = hf.spans[idx];
hf.spans[idx] = s;
}
return true;
}
/// @par
///
/// The span addition can be set to favor flags. If the span is merged to
/// another span and the new @p smax is within @p flagMergeThr units
/// from the existing span, the span flags are merged.
///
/// @see rcHeightfield, rcSpan.
bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y,
const unsigned short smin, const unsigned short smax,
const unsigned char area, const int flagMergeThr)
{
rcAssert(ctx);
if (!addSpan(hf, x, y, smin, smax, area, flagMergeThr))
{
ctx->log(RC_LOG_ERROR, "rcAddSpan: Out of memory.");
return false;
}
return true;
}
// divides a convex polygons into two convex polygons on both sides of a line
static void dividePoly(const float* in, int nin,
float* out1, int* nout1,
float* out2, int* nout2,
float x, int axis)
{
float d[12];
for (int i = 0; i < nin; ++i)
d[i] = x - in[i*3+axis];
int m = 0, n = 0;
for (int i = 0, j = nin-1; i < nin; j=i, ++i)
{
bool ina = d[j] >= 0;
bool inb = d[i] >= 0;
if (ina != inb)
{
float s = d[j] / (d[j] - d[i]);
out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
rcVcopy(out2 + n*3, out1 + m*3);
m++;
n++;
// add the i'th point to the right polygon. Do NOT add points that are on the dividing line
// since these were already added above
if (d[i] > 0)
{
rcVcopy(out1 + m*3, in + i*3);
m++;
}
else if (d[i] < 0)
{
rcVcopy(out2 + n*3, in + i*3);
n++;
}
}
else // same side
{
// add the i'th point to the right polygon. Addition is done even for points on the dividing line
if (d[i] >= 0)
{
rcVcopy(out1 + m*3, in + i*3);
m++;
if (d[i] != 0)
continue;
}
rcVcopy(out2 + n*3, in + i*3);
n++;
}
}
*nout1 = m;
*nout2 = n;
}
static bool rasterizeTri(const float* v0, const float* v1, const float* v2,
const unsigned char area, rcHeightfield& hf,
const float* bmin, const float* bmax,
const float cs, const float ics, const float ich,
const int flagMergeThr)
{
const int w = hf.width;
const int h = hf.height;
float tmin[3], tmax[3];
const float by = bmax[1] - bmin[1];
// Calculate the bounding box of the triangle.
rcVcopy(tmin, v0);
rcVcopy(tmax, v0);
rcVmin(tmin, v1);
rcVmin(tmin, v2);
rcVmax(tmax, v1);
rcVmax(tmax, v2);
// If the triangle does not touch the bbox of the heightfield, skip the triagle.
if (!overlapBounds(bmin, bmax, tmin, tmax))
return true;
// Calculate the footprint of the triangle on the grid's y-axis
int y0 = (int)((tmin[2] - bmin[2])*ics);
int y1 = (int)((tmax[2] - bmin[2])*ics);
y0 = rcClamp(y0, 0, h-1);
y1 = rcClamp(y1, 0, h-1);
// Clip the triangle into all grid cells it touches.
float buf[7*3*4];
float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3;
rcVcopy(&in[0], v0);
rcVcopy(&in[1*3], v1);
rcVcopy(&in[2*3], v2);
int nvrow, nvIn = 3;
for (int y = y0; y <= y1; ++y)
{
// Clip polygon to row. Store the remaining polygon as well
const float cz = bmin[2] + y*cs;
dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2);
rcSwap(in, p1);
if (nvrow < 3) continue;
// find the horizontal bounds in the row
float minX = inrow[0], maxX = inrow[0];
for (int i=1; i<nvrow; ++i)
{
if (minX > inrow[i*3]) minX = inrow[i*3];
if (maxX < inrow[i*3]) maxX = inrow[i*3];
}
int x0 = (int)((minX - bmin[0])*ics);
int x1 = (int)((maxX - bmin[0])*ics);
x0 = rcClamp(x0, 0, w-1);
x1 = rcClamp(x1, 0, w-1);
int nv, nv2 = nvrow;
for (int x = x0; x <= x1; ++x)
{
// Clip polygon to column. store the remaining polygon as well
const float cx = bmin[0] + x*cs;
dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0);
rcSwap(inrow, p2);
if (nv < 3) continue;
// Calculate min and max of the span.
float smin = p1[1], smax = p1[1];
for (int i = 1; i < nv; ++i)
{
smin = rcMin(smin, p1[i*3+1]);
smax = rcMax(smax, p1[i*3+1]);
}
smin -= bmin[1];
smax -= bmin[1];
// Skip the span if it is outside the heightfield bbox
if (smax < 0.0f) continue;
if (smin > by) continue;
// Clamp the span to the heightfield bbox.
if (smin < 0.0f) smin = 0;
if (smax > by) smax = by;
// Snap the span to the heightfield height grid.
unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, RC_SPAN_MAX_HEIGHT);
unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), (int)ismin+1, RC_SPAN_MAX_HEIGHT);
if (!addSpan(hf, x, y, ismin, ismax, area, flagMergeThr))
return false;
}
}
return true;
}
/// @par
///
/// No spans will be added if the triangle does not overlap the heightfield grid.
///
/// @see rcHeightfield
bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2,
const unsigned char area, rcHeightfield& solid,
const int flagMergeThr)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
const float ics = 1.0f/solid.cs;
const float ich = 1.0f/solid.ch;
if (!rasterizeTri(v0, v1, v2, area, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
{
ctx->log(RC_LOG_ERROR, "rcRasterizeTriangle: Out of memory.");
return false;
}
return true;
}
/// @par
///
/// Spans will only be added for triangles that overlap the heightfield grid.
///
/// @see rcHeightfield
bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
const int* tris, const unsigned char* areas, const int nt,
rcHeightfield& solid, const int flagMergeThr)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
const float ics = 1.0f/solid.cs;
const float ich = 1.0f/solid.ch;
// Rasterize triangles.
for (int i = 0; i < nt; ++i)
{
const float* v0 = &verts[tris[i*3+0]*3];
const float* v1 = &verts[tris[i*3+1]*3];
const float* v2 = &verts[tris[i*3+2]*3];
// Rasterize.
if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
{
ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}
/// @par
///
/// Spans will only be added for triangles that overlap the heightfield grid.
///
/// @see rcHeightfield
bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
const unsigned short* tris, const unsigned char* areas, const int nt,
rcHeightfield& solid, const int flagMergeThr)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
const float ics = 1.0f/solid.cs;
const float ich = 1.0f/solid.ch;
// Rasterize triangles.
for (int i = 0; i < nt; ++i)
{
const float* v0 = &verts[tris[i*3+0]*3];
const float* v1 = &verts[tris[i*3+1]*3];
const float* v2 = &verts[tris[i*3+2]*3];
// Rasterize.
if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
{
ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}
/// @par
///
/// Spans will only be added for triangles that overlap the heightfield grid.
///
/// @see rcHeightfield
bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt,
rcHeightfield& solid, const int flagMergeThr)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES);
const float ics = 1.0f/solid.cs;
const float ich = 1.0f/solid.ch;
// Rasterize triangles.
for (int i = 0; i < nt; ++i)
{
const float* v0 = &verts[(i*3+0)*3];
const float* v1 = &verts[(i*3+1)*3];
const float* v2 = &verts[(i*3+2)*3];
// Rasterize.
if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr))
{
ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}

1824
lib/Recast/RecastRegion.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3096
lib/gpc/gpc.cpp.h
View File

File diff suppressed because it is too large Load Diff

8
main.cpp
View File

@@ -83,16 +83,16 @@ void wifi() {
int main(int argc, char** argv) {
wifi(); return 0;
//wifi(); return 0;
#ifdef WITH_TESTS
::testing::InitGoogleTest(&argc, argv);
// skip all tests starting with LIVE_
//::testing::GTEST_FLAG(filter) = "*Barometer*";
//::testing::GTEST_FLAG(filter) = "*Barometer*";
::testing::GTEST_FLAG(filter) = "*Distribution*";
//::testing::GTEST_FLAG(filter) = "*Distribution.T*";
//::testing::GTEST_FLAG(filter) = "*RingBuffer*";
//::testing::GTEST_FLAG(filter) = "*Grid.*";
@@ -109,7 +109,7 @@ int main(int argc, char** argv) {
//::testing::GTEST_FLAG(filter) = "*Matrix4*";
//::testing::GTEST_FLAG(filter) = "*Sphere3*";
//::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*";
::testing::GTEST_FLAG(filter) = "NavMesh*";
//::testing::GTEST_FLAG(filter) = "Timestamp*";
//::testing::GTEST_FLAG(filter) = "*RayTrace3*";

11
math/boxkde/BoxGaus.h
View File

@@ -3,6 +3,7 @@
#include <cmath>
#include <vector>
#include "Image2D.h"
#include "BoxSizes.h"
template <class T>
@@ -28,7 +29,7 @@ struct BoxGaus
{
BoxSizes<T> bsX(sigmaX, nFilt);
BoxSizes<T> bsY(sigmaY, nFilt);
std::vector<T> buffer(input.size());
std::vector<T> buffer(input.size());
assertMsg((2 * bsX.wl + 1 < w) && (2 * bsX.wl + 1 < h), "Box-Filter size in X direction is too big");
assertMsg((2 * bsX.wu + 1 < w) && (2 * bsX.wu + 1 < h), "Box-Filter size in X direction is too big");
@@ -89,7 +90,7 @@ private:
for (size_t j = 0; j <= r; j++)
{
val += src[ri] - fv;
dst[j + i*w] = val * iarr;
dst[i + j*w] = val * iarr;
ri += w;
ti += w;
@@ -99,7 +100,7 @@ private:
for (size_t j = r + 1; j < h - r; j++)
{
val += src[ri] - src[li];
dst[j + i*w] = val * iarr;
dst[i + j*w] = val * iarr;
li += w;
ri += w;
@@ -110,12 +111,14 @@ private:
for (size_t j = h - r; j < h; j++)
{
val += lv - src[li];
dst[j + i*w] = val * iarr;
dst[i + j*w] = val * iarr;
li += w;
ti += w;
}
}
int test = 0;
}
};

1
math/boxkde/Image2D.h
View File

@@ -174,7 +174,6 @@ struct Image2D : public ImageView2D<TValue>
assertMsg(data.size() == width*height, "Sizes must be the same");
this->values = values_vec.data();
}
std::vector<TValue>& data() { return values_vec; }
const std::vector<TValue>& data() const { return values_vec; }

5
math/boxkde/benchmark.h
View File

@@ -2,6 +2,9 @@
#include <chrono>
#include <string>
#include <iostream>
#include <vector>
#include <algorithm>
#include <numeric>
template <typename F>
static void benchmark(std::string name, size_t count, F&& lambda)
@@ -102,4 +105,4 @@ public:
lapTime = clock::now();
}
};
};

5
math/distribution/Normal.h
View File

@@ -26,8 +26,13 @@ namespace Distribution {
Normal(const T mu, const T sigma) :
mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(RANDOM_SEED), dist(mu,sigma) {
#warning "analyze issue when coping an existing distribution and using draw() afterwards. this seems to yield issues"
}
/** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */
//Normal(const Normal& o) = delete;
/** get probability for the given value */
T getProbability(const T val) const {
const T b = -0.5 * ((val-mu)/sigma) * ((val-mu)/sigma);

63
math/speed.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef SPEED_H
#define SPEED_H
#include <cmath>
class Speed {
public:
#define PI_FLOAT 3.14159265f
#define PIBY2_FLOAT 1.5707963f
static inline float atan2(float y, float x) {
//http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html
//Volkan SALMA
const float ONEQTR_PI = M_PI / 4.0;
const float THRQTR_PI = 3.0 * M_PI / 4.0;
float r, angle;
float abs_y = fabs(y) + 1e-10f; // kludge to prevent 0/0 condition
if ( x < 0.0f ) {
r = (x + abs_y) / (abs_y - x);
angle = THRQTR_PI;
} else {
r = (x - abs_y) / (x + abs_y);
angle = ONEQTR_PI;
}
angle += (0.1963f * r * r - 0.9817f) * r;
if ( y < 0.0f )
return( -angle ); // negate if in quad III or IV
else
return( angle );
}
// // https://gist.github.com/volkansalma/2972237
// static inline float atan2(const float y, const float x) {
// if ( x == 0.0f ) {
// if ( y > 0.0f ) return PIBY2_FLOAT;
// if ( y == 0.0f ) return 0.0f;
// return -PIBY2_FLOAT;
// }
// float atan;
// float z = y/x;
// if ( fabs( z ) < 1.0f ) {
// atan = z/(1.0f + 0.28f*z*z);
// if ( x < 0.0f ) {
// if ( y < 0.0f ) return atan - PI_FLOAT;
// return atan + PI_FLOAT;
// }
// } else {
// atan = PIBY2_FLOAT - z/(z*z + 0.28f);
// if ( y < 0.0f ) return atan - PI_FLOAT;
// }
// return atan;
// }
};
#endif // SPEED_H

130
misc/PerfCheck.h Normal file
View File

@@ -0,0 +1,130 @@
#ifndef PERFCHECK_H
#define PERFCHECK_H
//#define WITH_PERF_CHECK
#ifdef WITH_PERF_CHECK
#include <string>
#include <time.h>
#include <unordered_map>
class PerfCheck {
struct Stats {
size_t calls = 0;
clock_t time = 0;
};
uint32_t name;
clock_t in;
static Stats stats[1024];
public:
/** ctor */
explicit PerfCheck(const uint32_t name) : name(name), in(clock()) {
;
}
/** dtor */
~PerfCheck() {
stats[name].calls += 1;
stats[name].time += clock() - in;
}
PerfCheck(PerfCheck const&) = delete;
PerfCheck& operator = (PerfCheck const&) = delete;
static void dump() {
for (int i = 0; i < 1024; ++i) {
const Stats& s = stats[i];
if (s.calls != 0) {
std::cout << i << ":\t";
std::cout << "\tcalls: " << s.calls;
std::cout << "\ttime: " << s.time;
std::cout << "\ttime/call: " << ((double)s.time / (double)s.calls);
std::cout << std::endl;
}
}
}
// static inline Stats* map() {
// static Stats stats[1024];// = new Stats[1024];
// return stats;
// }
};
PerfCheck::Stats PerfCheck::stats[1024];
#define PERF_REGION(idx, name) PerfCheck pcr_idx(idx)
#define PERF_DUMP() PerfCheck::dump();
#else
#define PERF_REGION(idx, name)
#define PERF_DUMP()
#endif
/*
class PerfCheck {
std::string name;
clock_t in;
public:
explicit PerfCheck(const std::string& name) : name(name), in(clock()) {
;
}
~PerfCheck() {
const clock_t diff = (clock() - in);
add(name, diff);
}
PerfCheck(PerfCheck const&) = delete;
PerfCheck& operator = (PerfCheck const&) = delete;
static void dump() {
for (const auto& it : map()) {
std::cout << it.first << ":\t";
std::cout << "\tcalls: " << it.second.calls;
std::cout << "\ttime: " << it.second.time;
std::cout << "\ttime/call: " << ((double)it.second.time / (double)it.second.calls);
std::cout << std::endl;
}
}
private:
struct Stats {
size_t calls = 0;
clock_t time = 0;
};
static std::unordered_map<std::string, Stats>& map() {
static std::unordered_map<std::string, Stats> stats;
return stats;
}
void add(const std::string& name, const clock_t diff) {
std::unordered_map<std::string, Stats>& m = map();
m[name].calls += 1;
m[name].time += diff;
}
};
*/
#endif // PERFCHECK_H

121
navMesh/NavMesh.h Normal file
View File

@@ -0,0 +1,121 @@
#ifndef NAV_MESH_H
#define NAV_MESH_H
#include "NavMeshTriangle.h"
#include <vector>
#include "../geo/BBox3.h"
#include <random>
#include "../math/DrawList.h"
#include "NavMeshRandom.h"
#include "NavMeshLocation.h"
namespace NM {
template <typename Tria> class NavMesh {
/** all triangles within the mesh */
std::vector<Tria*> triangles;
BBox3 bbox;
public:
/** ctor */
NavMesh() {
}
/** dtor */
~NavMesh() {
for (const Tria* t : triangles) {delete t;}
triangles.clear();
}
/** the overall bounding-box */
const BBox3 getBBox() const {
return bbox;
}
/** add a new triangle */
void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
triangles.push_back(new Tria(p1,p2,p3,type));
bbox.add(p1);
bbox.add(p2);
bbox.add(p3);
}
/** get the triangle this point belongs to (if any) */
NavMeshLocation<Tria> getLocation(const Point3 pos) {
for (const Tria* tria : triangles) {
if (tria->contains(pos)) {
return NavMeshLocation<Tria>(pos, tria);
}
}
throw Exception("location not found within NavMesh: " + pos.asString());
}
/** connect both triangles */
void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2);
connectUniDir(idx2,idx1);
}
/** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) {
Tria* tria = triangles[idxFrom];
tria->addNeighbor(triangles[idxTo]);
}
/** allows for-each iteration over all included triangles */
decltype(triangles.begin()) begin() {return triangles.begin();}
/** allows for-each iteration over all included triangles */
decltype(triangles.end()) end() {return triangles.end();}
/** array access */
Tria* operator [] (const size_t idx) {
Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
return triangles[idx];
}
/** get the number of triangles used */
size_t getNumTriangles() const {
return triangles.size();
}
/** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandom() {
return NavMeshRandom<Tria>(triangles);
}
// /** ---------------- NEIGHBORS ---------------- */
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const size_t idx) const {
// return getNumNeighbors(triangles[idx]);
// }
// /** get the number of neighbors for the given element */
// int getNumNeighbors(const Tria& e) const {
// return e._numNeighbors;
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
// const Tria& node = triangles[nodeIdx];
// return getNeighbor(node, nth);
// }
// /** get the n-th neighbor for the given node */
// Tria& getNeighbor(const Tria& tria, const size_t nth) const {
// const Tria& neighbor = triangles[tria._neighbors[nth]];
// return (Tria&) neighbor;
// }
};
}
#endif

128
navMesh/NavMeshDebug.h Normal file
View File

@@ -0,0 +1,128 @@
#ifndef NAVMESHDEBUG_H
#define NAVMESHDEBUG_H
#include "NavMesh.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
namespace NM {
/**
* debug plot NavMeshes
*/
class NavMeshDebug {
public:
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementPoints border;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines pathEstimated;
private:
K::GnuplotFill gFill[6] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed
};
public:
NavMeshDebug() {
gp << "set view equal xy\n";
plot.add(&lines); lines.setShowPoints(true);
plot.add(&border);
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
}
void draw() {
gp.draw(plot);
gp.flush();
}
template <typename T> void showParticles(const std::vector<T>& particles) {
this->particles.clear();
double min = +999;
double max = -999;
for (const T& p : particles) {
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const double prob = std::pow(p.weight, 0.25);
this->particles.add(p3, prob);
if (prob > max) {max = prob;}
if (prob < min) {min = prob;}
}
plot.getAxisCB().setRange(min, max + 0.000001);
}
template <typename Tria> void addMesh(NavMesh<Tria>& nm) {
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
const BBox3 bbox = nm.getBBox();
border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
border.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
//stairs in eigene group? vlt gehen dann auch die dellen weg?
for (const Tria* tria : nm) {
const uint8_t type = tria->getType();
if (type < 0 || type > 5) {
throw std::runtime_error("out of type-bounds");
}
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
pol->close();
pol->setZIndex(tria->getP3().z);
plot.getObjects().add(pol);
//for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
// const Tria* o = nm.getNeighbor(tria, i);
// const Point3 p1 = tria->getCenter();
// const Point3 p2 = o.getCenter();
// //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
//}
for (const NavMeshTriangle* o : *tria) {
const Point3 p1 = tria->getCenter();
const Point3 p2 = o->getCenter();
// lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
}
}
plot.getObjects().reOrderByZIndex();
}
void setGT(const Point3 pt) {
gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
}
void setCurPos(const Point3 pt) {
gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n";
}
};
}
#endif // NAVMESHDEBUG_H

745
navMesh/NavMeshFactory.h Normal file
View File

@@ -0,0 +1,745 @@
#ifndef NAV_MESH_FACTORY_H
#define NAV_MESH_FACTORY_H
#include "../floorplan/v2/Floorplan.h"
#include "../floorplan/v2/FloorplanHelper.h"
#include "NavMesh.h"
#include "NavMeshTriangle.h"
#include "NavMeshFactoryListener.h"
#include "NavMeshType.h"
#include "NavMeshSettings.h"
#include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h"
namespace NM {
class NavMeshPoly {
struct GPCPolygon : gpc_polygon {
GPCPolygon() {
num_contours = 0;
contour = nullptr;
hole = nullptr;
}
~GPCPolygon() {
if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
}
free(contour); contour = nullptr;
free(hole); hole = nullptr;
}
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) {
this->contour = o.contour;
this->hole = o.hole;
this->num_contours = o.num_contours;
o.contour = nullptr;
o.hole = nullptr;
return *this;
}
};
private:
GPCPolygon state;
float z;
public:
NavMeshPoly(float z) : z(z) {
;
}
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
}
std::vector<std::vector<Point3>> get() {
gpc_tristrip res;
res.num_strips = 0;
res.strip = nullptr;
//res.strip = (gpc_vertex_list*) malloc(1024);
gpc_polygon_to_tristrip(&state, &res);
std::vector<std::vector<Point3>> trias;
for (int i = 0; i < res.num_strips; ++i) {
gpc_vertex_list lst = res.strip[i];
for (int j = 2; j < lst.num_vertices; ++j) {
std::vector<Point3> tria;
gpc_vertex& v1 = lst.vertex[j-2];
gpc_vertex& v2 = lst.vertex[j-1];
gpc_vertex& v3 = lst.vertex[j];
tria.push_back(Point3(v1.x, v1.y, z));
tria.push_back(Point3(v2.x, v2.y, z));
tria.push_back(Point3(v3.x, v3.y, z));
trias.push_back(tria);
}
}
gpc_free_tristrip(&res);
return std::move(trias);
}
private:
GPCPolygon toGPC(Floorplan::Polygon2 poly) {
std::vector<gpc_vertex> verts;
for (Point2 p2 : poly.points) {
gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
}
};
struct TriangleIn {
Point3 p1;
Point3 p2;
Point3 p3;
uint8_t type;
TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;}
};
struct TriangleOut {
Point3 p1;
Point3 p2;
Point3 p3;
int numNeighbors = 0;
int neighbors[3]; // each triangle has max 3 neighbors
TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;}
Point3 center() const {
return (p1+p2+p3) / 3;
}
};
#define NMF_STEPS 8
template <typename Tria> class NavMeshFactory {
private:
NavMesh<Tria>* dst = nullptr;
const NavMeshSettings& settings;
std::vector<TriangleIn> triangles;
public:
NavMeshFactory(NavMesh<Tria>* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) {
}
void build(Floorplan::IndoorMap* map, NavMeshFactoryListener* listener = nullptr) {
if (listener) {listener->onNavMeshBuildUpdateMajor("preparing");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 0);}
const BBox3 bbox = FloorplanHelper::getBBox(map);
for (const Floorplan::Floor* floor : map->floors) {
add(floor);
}
fire(bbox, listener);
}
// /** get the smallest obstacle size that can be detected */
// float getMaxQuality_m() const {
// return maxQuality_m;
// }
private:
/** add one floor */
void add(const Floorplan::Floor* floor) {
if (!floor->enabled) {return;}
// NavMeshPoly nmPoly(floor->atHeight);
// for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
// if (poly->method == Floorplan::OutlineMethod::ADD) {
// nmPoly.add(poly->poly);
// }
// }
// for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
// if (poly->method == Floorplan::OutlineMethod::REMOVE) {
// nmPoly.remove(poly->poly);
// }
// }
// for (Floorplan::FloorObstacle* obs : floor->obstacles) {
// Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
// if (line != nullptr) {
// nmPoly.remove(getPolygon(line));
// }
// }
// std::vector<std::vector<Point3>> tmp = nmPoly.get();
// for (const std::vector<Point3>& tria : tmp) {
// const TriangleIn t(tria[0], tria[1], tria[2], 1; // TODO outdoor
// triangles.push_back(t);
// }
// we need this strange loop, as we need to distinguish between indoor and outdoor regions/polygons
// adding all "add" polygons first and removing "remove" polygons / obstacles afterwards is more performant
// but does not allow for tagging the "add" polygons (indoor/outdoor/...)
// thats why we have to tread each "add" polygon on its own (and remove all potential elements from it)
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
// if this is a to-be-added polygon, add it
if (poly->method == Floorplan::OutlineMethod::ADD) {
NavMeshPoly nmPoly(floor->atHeight);
nmPoly.add(poly->poly);
// get all other polygons of this floor, that are tagged as "remove" and remove them (many will be outside of the added polygon)
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
if (poly->method == Floorplan::OutlineMethod::REMOVE) {
nmPoly.remove(poly->poly);
}
}
// get all obstacles of this floor and remove them from the polygon as well (many will be outside of the added polygon)
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
}
}
// construct and add
std::vector<std::vector<Point3>> tmp = nmPoly.get();
int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR;
for (const std::vector<Point3>& tria : tmp) {
const TriangleIn t(tria[0], tria[1], tria[2], type);
triangles.push_back(t);
}
}
}
// add all stairs
// those must be DIRECTLY connected to the ending floor (stair's ending edge connected to an edge of the floor)
// otherwise the stair ends UNDER a floor polygon and is thus not added (higher polygons always win)
for (const Floorplan::Stair* stair : floor->stairs) {
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor); // slightly grow to ensure connection?!
for (const Floorplan::Quad3& quad : quads) {
// stair has two options: either leveled parts (no steps) and skewed parts (steps)
// as those affect the pedestrian's step-length, we tag them differently
const int type = quad.isLeveled() ? (int) NavMeshType::STAIR_LEVELED : (int) NavMeshType::STAIR_SKEWED;
const TriangleIn t1(quad.p1, quad.p2, quad.p3, type);
const TriangleIn t2(quad.p1, quad.p3, quad.p4, type);
triangles.push_back(t1);
triangles.push_back(t2);
// sanity check. should never happen. just to be ultra sure
const Point3 norm1 = cross((t1.p2-t1.p1), (t1.p3-t1.p1));
const Point3 norm2 = cross((t2.p2-t2.p1), (t2.p3-t2.p1));
Assert::isTrue(norm1.z > 0, "detected invalid culling for stair-quad. normal points downwards");
Assert::isTrue(norm2.z > 0, "detected invalid culling for stair-quad. normal points downwards");
}
}
// finally create additional triangles for the doors to tag doors differently (tagging also seems to improve the triangulation result)
// note: door-regions are already walkable as doors are NOT removed from the outline
// however: adding them again here seems to work.. triangles at the end of the list seem to overwrite (tagging) previous ones -> fine
{
// add (overlay) all doors for tagging them within the plan
NavMeshPoly nmDoors(floor->atHeight);
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(obs);
if (door != nullptr) {
nmDoors.add(getPolygon(door));
}
}
// construct and add triangles
std::vector<std::vector<Point3>> tmp = nmDoors.get();
for (const std::vector<Point3>& tria : tmp) {
const TriangleIn t(tria[0], tria[1], tria[2], (int) NavMeshType::DOOR);
triangles.push_back(t);
}
}
}
bool fire(BBox3 bbox, NavMeshFactoryListener* listener) {
std::vector<int> tData;
std::vector<float> vData;
std::vector<uint8_t> typeData;
if (listener) {listener->onNavMeshBuildUpdateMajor("building polygons");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 1);}
// floor outlines
for (const TriangleIn& t : triangles) {
// swap YZ and polygon order
int startVert = vData.size() / 3;
// invert triangle ? (CW vs CCW)
// ensure normal points UP
const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1));
if (norm.z > 0) {
tData.push_back(startVert + 0);
tData.push_back(startVert + 2);
tData.push_back(startVert + 1);
} else {
tData.push_back(startVert + 0);
tData.push_back(startVert + 1);
tData.push_back(startVert + 2);
}
typeData.push_back(t.type);
vData.push_back(t.p1.x);
vData.push_back(t.p1.z);
vData.push_back(t.p1.y);
vData.push_back(t.p2.x);
vData.push_back(t.p2.z);
vData.push_back(t.p2.y);
vData.push_back(t.p3.x);
vData.push_back(t.p3.z);
vData.push_back(t.p3.y);
}
unsigned char* m_triareas = typeData.data();
const float* verts = vData.data();
const int* tris = tData.data();
int ntris = tData.size() / 3;
int nverts = vData.size() / 3;
//unsigned char* m_triareas;
rcHeightfield* m_solid;
rcCompactHeightfield* m_chf;
rcContourSet* m_cset;
rcPolyMesh* m_pmesh;
rcConfig m_cfg;
rcPolyMeshDetail* m_dmesh;
rcContext* m_ctx = new rcContext();
// float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m
// float m_cellHeight = maxQuality_m/2.0f; //0.2f;
// float m_agentHeight = 1.8f;
// float m_agentRadius = 0.2f;//0.6f;
// float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail!
// float m_agentMaxSlope = 45.0f; // elevator???
// float m_regionMinSize = 2;//8;
// float m_regionMergeSize = 20;
// float m_edgeMaxLen = 10.0f; // maximal size for one triangle. too high = too many samples when walking!
// float m_edgeMaxError = 1.1f; //1.3f; // higher values allow joining some small triangles
// float m_vertsPerPoly = 3;//6.0f;
// float m_detailSampleDist = 6.0f;
// float m_detailSampleMaxError = 1.0f;//1.0f;
// int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS
// Init build configuration from GUI
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.cs = settings.getCellSizeXY();
m_cfg.ch = settings.getCellSizeZ();
m_cfg.walkableSlopeAngle = settings.agentMaxSlope;
m_cfg.walkableHeight = (int)ceilf(settings.agentHeight / m_cfg.ch);
m_cfg.walkableClimb = (int)floorf(settings.getMaxClimb() / m_cfg.ch);
m_cfg.walkableRadius = (int)ceilf(settings.agentRadius / m_cfg.cs);
m_cfg.maxEdgeLen = (int)(settings.edgeMaxLen / settings.getCellSizeXY());
m_cfg.maxSimplificationError = settings.edgeMaxError;
m_cfg.minRegionArea = (int)rcSqr(settings.regionMinSize); // Note: area = size*size
m_cfg.mergeRegionArea = (int)rcSqr(settings.regionMergeSize); // Note: area = size*size
m_cfg.maxVertsPerPoly = settings.vertsPerPoly;
m_cfg.detailSampleDist = settings.detailSampleDist < 0.9f ? 0 : settings.getCellSizeXY() * settings.detailSampleDist;
m_cfg.detailSampleMaxError = settings.getCellSizeZ() * settings.detailSampleMaxError;
float bmin[3] = {bbox.getMin().x, bbox.getMin().z, bbox.getMin().y};
float bmax[3] = {bbox.getMax().x, bbox.getMax().z, bbox.getMax().y};// x/z swapped?
// Set the area where the navigation will be build.
// Here the bounds of the input mesh are used, but the
// area could be specified by an user defined box, etc.
rcVcopy(m_cfg.bmin, bmin);
rcVcopy(m_cfg.bmax, bmax);
rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height);
// Reset build times gathering.
m_ctx->resetTimers();
// Start the build process.
m_ctx->startTimer(RC_TIMER_TOTAL);
m_ctx->log(RC_LOG_PROGRESS, "Building navigation:");
m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f);
//
// Step 2. Rasterize input polygon soup.
//
if (listener) {listener->onNavMeshBuildUpdateMajor("rasterizing polygons");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 2);}
// Allocate voxel heightfield where we rasterize our input data to.
m_solid = rcAllocHeightfield();
if (!m_solid) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
return false;
}
if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
return false;
}
// Allocate array that can hold triangle area types.
// If you have multiple meshes you need to process, allocate
// and array which can hold the max number of triangles you need to process.
// m_triareas = new unsigned char[ntris];
// if (!m_triareas)
// {
// m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris);
// return false;
// }
// Find triangles which are walkable based on their slope and rasterize them.
// If your input data is multiple meshes, you can transform them here, calculate
// the are type for each of the meshes and rasterize them.
//memset(m_triareas, 0, ntris*sizeof(unsigned char));
//rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas);
if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles.");
return false;
}
bool m_keepInterResults = false;
bool m_filterLowHangingObstacles = false;
bool m_filterLedgeSpans = false;
bool m_filterWalkableLowHeightSpans = false;
// Step 3. Filter walkables surfaces.
if (listener) {listener->onNavMeshBuildUpdateMajor("filtering");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 3);}
// Once all geoemtry is rasterized, we do initial pass of filtering to
// remove unwanted overhangs caused by the conservative rasterization
// as well as filter spans where the character cannot possibly stand.
if (m_filterLowHangingObstacles)
rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid);
if (m_filterLedgeSpans)
rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid);
if (m_filterWalkableLowHeightSpans)
rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid);
// Step 4. Partition walkable surface to simple regions.
if (listener) {listener->onNavMeshBuildUpdateMajor("partitioning");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 4);}
// Compact the heightfield so that it is faster to handle from now on.
// This will result more cache coherent data as well as the neighbours
// between walkable cells will be calculated.
m_chf = rcAllocCompactHeightfield();
if (!m_chf) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
return false;
}
if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
return false;
}
//if (!m_keepInterResults) {
rcFreeHeightField(m_solid);
m_solid = 0;
//}
// Erode the walkable area by agent radius.
if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
return false;
}
// (Optional) Mark areas.
// const ConvexVolume* vols = m_geom->getConvexVolumes();
// for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i)
// rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
// Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas.
// There are 3 martitioning methods, each with some pros and cons:
// 1) Watershed partitioning
// - the classic Recast partitioning
// - creates the nicest tessellation
// - usually slowest
// - partitions the heightfield into nice regions without holes or overlaps
// - the are some corner cases where this method creates produces holes and overlaps
// - holes may appear when a small obstacles is close to large open area (triangulation can handle this)
// - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail
// * generally the best choice if you precompute the nacmesh, use this if you have large open areas
// 2) Monotone partioning
// - fastest
// - partitions the heightfield into regions without holes and overlaps (guaranteed)
// - creates long thin polygons, which sometimes causes paths with detours
// * use this if you want fast navmesh generation
// 3) Layer partitoining
// - quite fast
// - partitions the heighfield into non-overlapping regions
// - relies on the triangulation code to cope with holes (thus slower than monotone partitioning)
// - produces better triangles than monotone partitioning
// - does not have the corner cases of watershed partitioning
// - can be slow and create a bit ugly tessellation (still better than monotone)
// if you have large open areas with small obstacles (not a problem if you use tiles)
// * good choice to use for tiled navmesh with medium and small sized tiles
switch (settings.partitionType) {
case SamplePartitionType::SAMPLE_PARTITION_WATERSHED:
// Prepare for region partitioning, by calculating distance field along the walkable surface.
if (!rcBuildDistanceField(m_ctx, *m_chf)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
return false;
}
// Partition the walkable surface into simple regions without holes.
if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions.");
return false;
}
break;
case SamplePartitionType::SAMPLE_PARTITION_MONOTONE:
// Partition the walkable surface into simple regions without holes.
// Monotone partitioning does not need distancefield.
if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions.");
return false;
}
break;
case SamplePartitionType::SAMPLE_PARTITION_LAYERS:
// Partition the walkable surface into simple regions without holes.
if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions.");
return false;
}
break;
default:
throw Exception("unsupported SamplePartitionType");
}
// Step 5. Trace and simplify region contours.
if (listener) {listener->onNavMeshBuildUpdateMajor("tracing");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 5);}
// Create contours.
m_cset = rcAllocContourSet();
if (!m_cset) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
return false;
}
if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
return false;
}
//
// Step 6. Build polygons mesh from contours.
if (listener) {listener->onNavMeshBuildUpdateMajor("building triangles");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 6);}
// Build polygon navmesh from the contours.
m_pmesh = rcAllocPolyMesh();
if (!m_pmesh) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
return false;
}
if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
return false;
}
//
// Step 7. Create detail mesh which allows to access approximate height on each polygon.
if (listener) {listener->onNavMeshBuildUpdateMajor("building details");}
if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 7);}
m_dmesh = rcAllocPolyMeshDetail();
if (!m_dmesh) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
return false;
}
if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) {
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
return false;
}
//if (!m_keepInterResults) {
rcFreeCompactHeightfield(m_chf);
m_chf = 0;
rcFreeContourSet(m_cset);
m_cset = 0;
//}
// std::vector<TriangleOut> res;
const float* orig = m_pmesh->bmin;
// https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt
for (int i = 0; i < m_pmesh->npolys; ++i) {
const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2];
const uint8_t type = m_pmesh->areas[i];
// Each entry is <tt>2 * #nvp</tt> in length. The first half of the entry
// contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX
// indicates the end of the indices for the entry. The second half contains
// indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no
// connection for the associated edge. (I.e. The edge is a solid border.)
// we only use exactly 3 vertices per polygon, no iteration needed
// for (int j = 0; j < m_pmesh->nvp; ++j) {
// if (p[j] == RC_MESH_NULL_IDX) {break;}
// const unsigned short* v = &m_pmesh->verts[p[j]*3];
// const float x = orig[0] + v[0]*m_pmesh->cs;
// const float z = orig[1] + v[1]*m_pmesh->ch;
// const float y = orig[2] + v[2]*m_pmesh->cs;
// pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST));
// }
// un-swap Y/Z
const unsigned short* v0 = &m_pmesh->verts[p[0]*3];
const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch);
const unsigned short* v1 = &m_pmesh->verts[p[1]*3];
const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch);
const unsigned short* v2 = &m_pmesh->verts[p[2]*3];
const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch);
dst->add(p0,p1,p2,type);
}
// now, connect neighbors
for (int i = 0; i < m_pmesh->npolys; ++i) {
const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2];
// find all neighbor polygons using their index
for (int j = 0; j < m_pmesh->nvp; ++j) {
int jj = j + m_pmesh->nvp; // offset, 2nd half of the array [size: 2*nvp]
if (p[jj] == RC_MESH_NULL_IDX) {continue;} // no neighbor for the current edge!
const int idx = p[jj];
dst->connectUniDir(i, idx);
}
}
return true;
}
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const {
const float thickness_m = std::max(line->thickness_m, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected)
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
Floorplan::Polygon2 res;
res.points.push_back(p1);
res.points.push_back(p2);
res.points.push_back(p4);
res.points.push_back(p3);
return res;
}
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleDoor* door) const {
const float thickness_m = std::max(0.3f, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected)
const Point2 dir = (door->to - door->from); // obstacle's direction
const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree)
const Point2 p1 = door->from + perp * thickness_m/2; // start-up
const Point2 p2 = door->from - perp * thickness_m/2; // start-down
const Point2 p3 = door->to + perp * thickness_m/2; // end-up
const Point2 p4 = door->to - perp * thickness_m/2; // end-down
Floorplan::Polygon2 res;
res.points.push_back(p1);
res.points.push_back(p2);
res.points.push_back(p4);
res.points.push_back(p3);
return res;
}
};
}
#endif

20
navMesh/NavMeshFactoryListener.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef NAVMESHFACTORYLISTENER_H
#define NAVMESHFACTORYLISTENER_H
#include <string>
namespace NM {
/** listen for events during the build process */
class NavMeshFactoryListener {
public:
virtual void onNavMeshBuildUpdateMajor(const std::string& what) = 0;
virtual void onNavMeshBuildUpdateMajor(const int cnt, const int cur) = 0;
};
}
#endif // NAVMESHFACTORYLISTENER_H

37
navMesh/NavMeshLocation.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef NAVMESHLOCATION_H
#define NAVMESHLOCATION_H
#include "../geo/Point3.h"
class NavMeshTriangle;
namespace NM {
/**
* as Point3 -> Triangle (on Mesh) lookups are expensive,
* we try to combine both information (point -> triangle)
* most of the time using this structure
*/
template <typename Tria> struct NavMeshLocation {
/** point within the world (in meter) */
Point3 pos;
/** NavMeshTriangle the point belongs to */
const Tria* tria;
/** empty ctor */
NavMeshLocation() : pos(0,0,0), tria(nullptr) {
;
}
/** ctor */
NavMeshLocation(const Point3 pos, const Tria* tria) : pos(pos), tria(tria) {
;
}
};
}
#endif // NAVMESHLOCATION_H

112
navMesh/NavMeshRandom.h Normal file
View File

@@ -0,0 +1,112 @@
#ifndef NAVMESHRANDOM_H
#define NAVMESHRANDOM_H
#include <random>
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
#include "../misc/PerfCheck.h"
#include "NavMeshLocation.h"
namespace NM {
/**
* randomly pick points within the area of the nav-mesh.
* points are picked evenly:
* bigger triangles are used more often
*
*/
template <typename Tria> class NavMeshRandom {
DrawList<size_t> lst;
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2);
std::vector<const Tria*> triangles;
uint32_t nextSeed() {
static uint32_t seed = 0;
return ++seed;
}
public:
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
// 1st = almost always the same number?!
gen(); gen();
// construct a DrawList (probability = size[area] of the triangle
// bigger triangles must be choosen more often
for (size_t idx = 0; idx < srcTriangles.size(); ++idx) {
this->triangles.push_back(srcTriangles[idx]);
this->lst.add(idx, srcTriangles[idx]->getArea());
}
}
/** draw a random point */
NavMeshLocation<Tria> draw() {
PERF_REGION(3, "NavMeshRandom::draw()");
// pick a random triangle to draw from
const size_t idx = lst.get();
const Tria* tria = triangles[idx];
// get random (u,v) on triangle
float u = dOnTriangle(gen);
float v = dOnTriangle(gen);
// if the (u,v) is outside of the triangle, mirror it so its inside the triangle again
if ((u+v) > 1) {
u = 1.0f - u;
v = 1.0f - v;
}
// done
const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria);
}
/** draw a random location within the given radius */
NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) {
std::uniform_real_distribution<float> dDistance(0.001, radius);
while(true) {
const float head = dHeading(gen);
const float dist = dDistance(gen);
const float ox = std::cos(head) * dist;
const float oy = std::sin(head) * dist;
// 2D destination (ignore z)
const Point2 dst(center.x + ox, center.y + oy);
for (const Tria* t : triangles) {
// if triangle contains 2D position
if (t->contains(dst)) {
// convert it to a 3D position
const Point3 p3 = t->toPoint3(dst);
const NavMeshLocation<Tria> loc(p3, t);
return loc;
}
}
}
}
};
}
#endif // NAVMESHRANDOM_H

63
navMesh/NavMeshSettings.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef NAVMESHSETTINGS_H
#define NAVMESHSETTINGS_H
namespace NM {
enum class SamplePartitionType {
SAMPLE_PARTITION_WATERSHED,
SAMPLE_PARTITION_MONOTONE,
SAMPLE_PARTITION_LAYERS,
};
struct NavMeshSettings {
/** maximum resolution for outputs. nothing below this size will be detected (walls, doors, ..) */
float maxQuality_m = 0.20f;
/** height of the walking person (used to delete regions below other regions) */
float agentHeight = 1.8f;
/** radius of the walking person (used to shrink the walkable area) */
float agentRadius = 0.2f;
/** the max angle (degree) the pedestrian is able to walk */
float agentMaxSlope = 45.0f; // elevator???
/** maximal size for one triangle. too high = too many samples when walking! */
float edgeMaxLen = 10.0f;
/** higher values allow joining some small triangles */
float edgeMaxError = 1.1f; //1.3f;
/** algorithm choice */
SamplePartitionType partitionType = SamplePartitionType::SAMPLE_PARTITION_WATERSHED;
const float regionMinSize = 2;//8; // (isolated) regions smaller than this will not be rendered?!
const float regionMergeSize = 20; //??
const int vertsPerPoly = 3;//6.0f;
const float detailSampleDist = 6.0f;
const float detailSampleMaxError = 1.0f;//1.0f;
float getCellSizeXY() const {
return maxQuality_m / 2.0f;
}
float getCellSizeZ() const {
return maxQuality_m / 2.0f;
}
/** allow jumping onto stairs from the side. usually we do not want this -> set it as low as possible */
float getMaxClimb() const {
return maxQuality_m; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail!
}
};
}
#endif // NAVMESHSETTINGS_H

230
navMesh/NavMeshTriangle.h Normal file
View File

@@ -0,0 +1,230 @@
#ifndef NAVMESHTRIANGLE_H
#define NAVMESHTRIANGLE_H
#include "../geo/Point3.h"
#include "../geo/Point2.h"
namespace NM {
/**
* represents one triangle within the NavMesh
* each Triangle has up to 3 neighbors (one per edge)
*
* for performance enhancements,
* some memeber attributes are pre-calculated once
*/
class NavMeshTriangle {
private:
template<typename> friend class NavMesh;
const Point3 p1;
const Point3 p2;
const Point3 p3;
const uint8_t type;
NavMeshTriangle* _neighbors[3];
int _numNeighbors;
private: // precalculated stuff
Point2 v0;
Point2 v1;
float dot00;
float dot01;
float dot11;
double invDenom;
float area;
float minZ;
float maxZ;
const Point3 center;
const Point3 v12;
const Point3 v13;
public:
/** ctor */
NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
precompute();
}
/** get the triangle's type */
uint8_t getType() const {return type;}
Point3 getP1() const {return p1;}
Point3 getP2() const {return p2;}
Point3 getP3() const {return p3;}
/** get the distance between the given point and the triangle using approximate tests */
float getDistanceApx(const Point3 pt) const {
// const float d1 = pt.getDistance(p1);
// const float d2 = pt.getDistance(p2);
// const float d3 = pt.getDistance(p3);
// const float d4 = pt.getDistance(center);
// const float d5 = pt.getDistance((p1-p2)/2);
// const float d6 = pt.getDistance((p2-p3)/2);
// const float d7 = pt.getDistance((p3-p1)/2);
// return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7))))));
// const float d1 = pt.getDistance(p1);
// const float d2 = pt.getDistance(p2);
// const float d3 = pt.getDistance(p3);
// const float d4 = pt.getDistance(center);
// return std::min(d1, std::min(d2, std::min(d3,d4)));
float bestD = 99999;
Point3 bestP;
Point3 dir12 = p2-p1;
Point3 dir13 = p3-p1;
Point3 dir23 = p3-p2;
for (float f = 0; f < 1; f += 0.05f) {
const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt);
const Point3 pos2 = p1 + dir13 * f; const float dist2 = pos2.getDistance(pt);
const Point3 pos3 = p2 + dir23 * f; const float dist3 = pos3.getDistance(pt);
if (dist1 < bestD) {bestP = pos1; bestD = dist1;}
if (dist2 < bestD) {bestP = pos2; bestD = dist2;}
if (dist3 < bestD) {bestP = pos3; bestD = dist3;}
}
return bestD;
}
bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
/** is the triangle plain? (same Z for all points) */
bool isPlain() const {
const float d1 = std::abs(p1.z - p2.z);
const float d2 = std::abs(p2.z - p3.z);
return (d1 < 0.1) && (d2 < 0.1);
}
const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
Point3 getPoint(const float u, const float v) const {
return p1 + (v12*u) + (v13*v);
}
/** does the triangle contain the given 3D point? */
bool contains(const Point3 p) const {
return (minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
}
/** does the triangle contain the given 2D point? */
bool contains(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// Check if point is in triangle
return (u >= 0) && (v >= 0) && (u + v <= 1);
}
/** estimate the correct z-value for the given 2D point */
Point3 toPoint3(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
const Point3 res = getPoint(v,u);
Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");
//return res;
return Point3(p.x, p.y, res.z); // only use the new z, keep input as-is
}
/** get the triangle's size */
float getArea() const {
return area;
}
/** get the triangle's center-point */
Point3 getCenter() const {
return center;
}
private:
/** perform some pre-calculations to speed things up */
void precompute() {
#warning "TODO, z buffer"
minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15; // TODO the builder does not align on the same height as we did
maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;
// Compute vectors
v0 = p3.xy() - p1.xy();
v1 = p2.xy() - p1.xy();
// Compute dot products
dot00 = dot(v0, v0);
dot01 = dot(v0, v1);
dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);
const float a = (p2-p1).length();
const float b = (p3-p1).length();
const float c = (p2-p3).length();
const float s = 0.5f * (a+b+c);
area = std::sqrt( s * (s-a) * (s-b) * (s-c) );
}
protected:
void addNeighbor(NavMeshTriangle* o) {
Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
_neighbors[_numNeighbors] = o;
++_numNeighbors;
}
};
}
#endif // NAVMESHTRIANGLE_H

22
navMesh/NavMeshType.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef NAVMESHTYPE_H
#define NAVMESHTYPE_H
namespace NM {
enum class NavMeshType {
UNWALKABLE, // needed by Recast
FLOOR_INDOOR,
FLOOR_OUTDOOR,
DOOR,
STAIR_LEVELED, // eben
STAIR_SKEWED, // schraeg
ELEVATOR,
};
}
#endif // NAVMESHTYPE_H

103
navMesh/walk/NavMeshSub.h Normal file
View File

@@ -0,0 +1,103 @@
#ifndef NAVMESHSUB_H
#define NAVMESHSUB_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../NavMeshRandom.h"
#include <vector>
#include <unordered_set>
namespace NM {
template <typename Tria> class NavMeshSub {
std::vector<const Tria*> toVisit;
public:
NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
build(loc,radius_m);
}
/** does this submesh contain the given point? */
bool contains(const Point2 p2) const {
for (const Tria* t : toVisit) {
if (t->contains(p2)) {return true;}
}
return false;
}
/** does this submesh contain the given point? */
bool contains(const Point3 p3) const {
for (const Tria* t : toVisit) {
if (t->contains(p3)) {return true;}
}
return false;
}
/** get the triangle that contains the given point (if any) */
const Tria* getContainingTriangle(const Point2 p2) const {
for (const Tria* t : toVisit) {
if (t->contains(p2)) {return t;}
}
return nullptr;
}
/** perform random operations on the submesh */
NavMeshRandom<Tria> getRandom() const {
return NavMeshRandom<Tria>(toVisit);
}
/** allows for-each iteration over all included triangles */
decltype(toVisit.begin()) begin() {return toVisit.begin();}
/** allows for-each iteration over all included triangles */
decltype(toVisit.end()) end() {return toVisit.end();}
private:
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
PERF_REGION(6, "NavMeshSub::build()");
std::unordered_set<const Tria*> visited;
// starting-triangle + all its (max 3) neighbors
toVisit.push_back(loc.tria);
visited.insert(loc.tria);
// for (const auto* n : *loc.tria) {
// toVisit.push_back( (const Tria*)n );
// }
// size_t next = 1; // start with the first neighbor (skip starting triangle itself)
size_t next = 0;
while (next < toVisit.size()) {
// next triangle
const NavMeshTriangle* cur = toVisit[next]; ++next;
// neighbors
for (const auto* n : *cur) {
const Tria* t = (const Tria*) n;
//const float dist = loc.pos.getDistance(n->getCenter());
const float dist = n->getDistanceApx(loc.pos);
if (dist > radius_m) {continue;}
if (visited.find(t) != visited.end()) {continue;}
toVisit.push_back(t);
visited.insert(t);
}
}
}
};
}
#endif // NAVMESHSUB_H

141
navMesh/walk/NavMeshWalkEval.h Normal file
View File

@@ -0,0 +1,141 @@
#ifndef NAVMESHWALKEVAL_H
#define NAVMESHWALKEVAL_H
#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
#include "../../misc/PerfCheck.h"
namespace NM {
template <typename Tria> struct NavMeshPotentialWalk {
NavMeshWalkParams<Tria> requested;
NavMeshLocation<Tria> end;
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
;
}
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
;
}
};
/**
* evaluate a NavMeshWalk from -> to = probability
*/
template <typename Tria> class NavMeshWalkEval {
public:
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;
};
/**
* evaluate the difference between head(start,end) and the requested heading
*/
template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
const double sigma_rad;
const double kappa;
Distribution::VonMises<double> _dist;
Distribution::LUT<double> dist;
public:
// kappa = 1/var = 1/sigma^2
// https://en.wikipedia.org/wiki/Von_Mises_distribution
WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
;
}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
PERF_REGION(4, "WalkEvalHeadingStartEnd");
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
const float diff = head.getDiffHalfRAD(walk.requested.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
}
};
/**
* evaluate the difference between head(start,end) and the requested heading
*/
template <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {
const double sigma_rad;
Distribution::Normal<double> dist;
public:
WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), dist(0, sigma_rad) {
;
}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
PERF_REGION(4, "WalkEvalHeadingStartEnd");
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
const float diff = head.getDiffHalfRAD(walk.requested.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
}
};
/**
* evaluate the difference between distance(start, end) and the requested distance
*/
template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {
const double sigma;
const Distribution::Normal<double> dist;
public:
WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
PERF_REGION(5, "WalkEvalDistance");
const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
const float diff = walkedDistance_m - requestedDistance_m;
return dist.getProbability(diff);
//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
}
};
}
#endif // NAVMESHWALKEVAL_H

76
navMesh/walk/NavMeshWalkParams.h Normal file
View File

@@ -0,0 +1,76 @@
#ifndef NAVMESHWALKPARAMS_H
#define NAVMESHWALKPARAMS_H
#include "../../geo/Heading.h"
#include "../NavMeshLocation.h"
#include "../NavMeshType.h"
namespace NM {
/** configure pedestrian StepSizes */
struct StepSizes {
float stepSizeFloor_m = NAN;
float stepSizeStair_m = NAN;
bool isValid() const {
return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
}
template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
Assert::isTrue(isValid(), "invalid step-sizes given");
if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
return stepSizeStair_m * steps;
} else {
return stepSizeFloor_m * steps;
}
// if (start.tria->isPlain()) {
// return stepSizeFloor_m * steps;
// } else {
// return stepSizeStair_m * steps;
// }
}
};
/** configure walking from -> to */
template <typename Tria> struct NavMeshWalkParams {
/** walk starts here (pos/tria) */
NavMeshLocation<Tria> start;
/** direction to walk to */
Heading heading;
/** number of steps to walk */
int numSteps;
/** configuration for pedestrian's step-sizes */
StepSizes stepSizes;
/** empty ctor */
NavMeshWalkParams() : heading(0) {;}
/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
float getToBeWalkedDistance() const {
if (_toBeWalkedDistance != _toBeWalkedDistance) {
_toBeWalkedDistance = stepSizes.inMeter(numSteps, start);
}
return _toBeWalkedDistance;
}
private:
// precalc
mutable float _toBeWalkedDistance = NAN;
};
}
#endif // NAVMESHWALKPARAMS_H

146
navMesh/walk/NavMeshWalkRandom.h Normal file
View File

@@ -0,0 +1,146 @@
#ifndef NAVMESHWALKRANDOM_H
#define NAVMESHWALKRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* pick a truely random destination within the reachable area
* weight this area (evaluators)
* repeat this several times to find a robus destination
*/
template <typename Tria> class NavMeshWalkRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
ResultEntry res;
res.probability = 0;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
// draw a random destination
// is this destination within the reachable area? (triangles might be larger!)
pwalk.end = rnd.draw();
if (pwalk.end.pos.getDistance(params.start.pos) > toBeWalkedDistSafe) {
--i; continue;
}
// calculate the probability for this destination
const double p = eval(pwalk);
// better?
if (p > res.probability) {
res.location = pwalk.end;
res.probability = p;
}
}
// destination is known. update the heading
res.heading = Heading(params.start.pos.xy(), res.location.pos.xy());
return res;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
ResultList res;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
pwalk.end = rnd.drawWithin(params.start.pos, toBeWalkedDistSafe);
// calculate the probability for this destination
const double p = eval(pwalk);
ResultEntry re;
re.heading = Heading(params.start.pos.xy(), pwalk.end.pos.xy());
re.location = pwalk.end;
re.probability = p;
res.push_back(re);
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
PERF_REGION(2, "NavMeshWalkRandom::EvalLoop");
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKRANDOM_H

169
navMesh/walk/NavMeshWalkSemiRandom.h Normal file
View File

@@ -0,0 +1,169 @@
#ifndef NAVMESHWALKSEMIRANDOM_H
#define NAVMESHWALKSEMIRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* similar to NavMeshWalkRandom but:
* pick a semi random destination within the reachable area (requested distance/heading + strong deviation)
* if this destination is reachable:
* weight this area (evaluators)
* repeat this some times to find a robus destination
*/
template <typename Tria> class NavMeshWalkSemiRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : public std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkSemiRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
ResultEntry re;
NavMeshPotentialWalk<Tria> pwalk(params);
pwalk.end = reachable.getRandom().draw(); // to have at least a non-start solution
re.probability = eval(pwalk);
re.location = pwalk.end;
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
// better?
if (p > re.probability) {
re.location = pwalk.end;
re.probability = p;
re.heading = head;
}
}
}
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
ResultList res;
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshPotentialWalk<Tria> pwalk(params);
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
ResultEntry re;
re.location = pwalk.end;
re.probability = p;
re.heading = head;
res.push_back(re);
}
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKSEMIRANDOM_H

114
navMesh/walk/NavMeshWalkSimple.h Normal file
View File

@@ -0,0 +1,114 @@
#ifndef NAVMESHWALKSIMPLE_H
#define NAVMESHWALKSIMPLE_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
template <typename Tria> class NavMeshWalkSimple {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
int hits = 0;
int misses = 0;
public:
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
/** list of results */
using ResultList = std::vector<ResultEntry>;
public:
/** ctor */
NavMeshWalkSimple(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
ResultEntry re;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
// construct reachable region
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
re.heading = params.heading; // heading was OK -> keep
re.location.pos = dstTria->toPoint3(dst); // new destination position
re.location.tria = dstTria; // new destination triangle
++hits;
} else {
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destianation
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading
++misses;
}
const int total = (hits + misses);
if (total % 10000 == 0) {
std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
// calculate probability
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
re.probability *= p1;
}
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
return {getOne(params)};
}
};
}
#endif // NAVMESHWALKSIMPLE_H

8
sensors/imu/MagnetometerData.h
View File

@@ -36,7 +36,7 @@ struct MagnetometerData {
}
float magnitude() const {
return std::sqrt( x*x + y*y + z*z );
return std::sqrt( x*x + y*y + z*z );
}
MagnetometerData& operator += (const MagnetometerData& o) {
@@ -73,9 +73,9 @@ private:
};
namespace std {
inline MagnetometerData sqrt(const MagnetometerData& o) {
return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
}
inline MagnetometerData sqrt(const MagnetometerData& o) {
return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
}
}
#endif // INDOOR_IMU_MAGNETOMETERDATA_H

4
sensors/imu/StepDetection.h
View File

@@ -36,7 +36,7 @@ private:
Timestamp blockUntil;
bool waitForUp = false;
const Timestamp blockTime = Timestamp::fromMS(250); // 150-250 looks good
const Timestamp blockTime; // 150-250 looks good
const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing!
@@ -57,7 +57,7 @@ private:
public:
/** ctor */
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
#ifdef WITH_DEBUG_PLOT
gp << "set autoscale xfix\n";

58
sensors/offline/FilePlayer.h
View File

@@ -75,13 +75,25 @@ namespace Offline {
thread.join();
}
/** manual ticking */
int tickPos = 0;
void tick() {
const std::vector<Entry>& events = reader->getEntries();
const Entry& e = events[tickPos];
trigger(Timestamp::fromMS(e.ts), e);
++tickPos;
}
private:
/** background loop */
void loop() {
// get all sensor events from the offline file
const std::vector<Entry> events = reader->getEntries();
const std::vector<Entry>& events = reader->getEntries();
// reference time (system vs. first-event)
Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
@@ -104,23 +116,7 @@ namespace Offline {
if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
}
// event index
const size_t idx = e.idx;
#warning "some sensors todo:"
switch(e.type) {
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break;
case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
default: throw Exception("code error. found not-yet-implemented sensor");
}
trigger(ts, e);
}
@@ -129,8 +125,34 @@ namespace Offline {
}
void trigger(const Timestamp ts, const Entry& e) {
const int idx = e.idx;
#warning "some sensors todo:"
switch(e.type) {
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break;
case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
default: throw Exception("code error. found not-yet-implemented sensor");
}
}
};
}
#endif // FILEPLAYER_H

1
smc/smoothing/BackwardSimulation.h
View File

@@ -169,6 +169,7 @@ namespace SMC {
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
continue;
}
// compute weights using the transition model

16
smc/smoothing/FastKDESmoothing.h
View File

@@ -76,17 +76,17 @@ namespace SMC {
/** ctor */
FastKDESmoothing(int numParticles, const Floorplan::IndoorMap* map, const int gridsize_cm, const Point2 bandwith) {
this->numParticles = numParticles;
backwardParticles.reserve(numParticles);
firstFunctionCall = true;
//backwardParticles.reserve(numParticles);
this->firstFunctionCall = true;
const Point3 maxBB = FloorplanHelper::getBBox(map).getMax();
const Point3 minBB = FloorplanHelper::getBBox(map).getMin();
bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
const Point3 maxBB = FloorplanHelper::getBBox(map).getMax() * 100.0;
const Point3 minBB = FloorplanHelper::getBBox(map).getMin() * 100.0;
this->bb = BoundingBox<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
// Create histogram
size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_cm);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_cm);
grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->bandwith = bandwith;
}
@@ -186,7 +186,7 @@ namespace SMC {
State est = estimation->estimate(smoothedParticles);
estimatedStates.push_back(est);
return est;
continue;
}
// transition p(q_t+1* | q_t): so we are performing again a forward transition step.
@@ -215,7 +215,7 @@ namespace SMC {
smoothedParticles = forwardHistory.getParticleSet(i);
for(Particle<State> p : smoothedParticles){
p.weight = p.weight * grid.fetch(p.state.position.x_cm, p.state.position.y_cm);
Assert::isNot0(p.weight, "smoothed particle has zero weight");
//Assert::isNot0(p.weight, "smoothed particle has zero weight");
}
//normalization

19
synthetic/SyntheticPath.h
View File

@@ -4,18 +4,23 @@
#include "../math/Interpolator.h"
#include "../floorplan/v2/Floorplan.h"
#include "../floorplan/v2/FloorplanHelper.h"
#include "../floorplan/v2/FloorplanHelper.h"
/** allows interpolation along a synthetic path */
class SyntheticPath : private Interpolator<float, Point3> {
using Base = Interpolator<float, Point3>;
using Entry = Base::InterpolatorEntry;
const Floorplan::IndoorMap* map;
public:
/** create path using the given ground-truth points from the map */
void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {
this->map = map;
// get all ground-truth points from the map
auto gtps = FloorplanHelper::getGroundTruthPoints(map);
float dist = 0;
@@ -38,6 +43,8 @@ public:
return Base::getEntries();
}
/** smooth harsh angles */
void smooth(float delta = 1, int numRuns = 1) {
@@ -104,6 +111,18 @@ public:
return Base::get(distance);
}
bool doneAtDistance(const float distance) const {
return Base::getMaxKey() < distance;
}
/** at the given distance: are we walking on a plain surface or up/down? */
bool isPlain(const float distance) const {
const Point3 pos1 = getPosAfterDistance(distance);
const Point3 pos2 = getPosAfterDistance(distance + 0.1);
const float delta = std::abs(pos1.z - pos2.z);
return delta < 0.01;
}
};
#endif // INDOOR_SYNTEHTICPATH_H

49
synthetic/SyntheticSteps.h
View File

@@ -25,26 +25,39 @@ private:
/** the walker to listen to */
SyntheticWalker* walker;
/** the pedestrian's step-size (in meter) */
float stepSize_m = 0.7;
///** the pedestrian's step-size (in meter) */
//float stepSize_m = 0;
///** when walking stairs, the step size is much smaller */
//float stepSizeStair_m = 0;
float lastStepAtDistance = 0;
Timestamp refStepPattern;
Interpolator<Timestamp, AccelerometerData> stepPattern;
Interpolator<Timestamp, AccelerometerData> stepPatternPlain;
Interpolator<Timestamp, AccelerometerData> stepPatternStair;
Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);
int stepPatternPos = -1;
std::vector<Listener*> listeners;
//float stepSize_m;
//float stepSizeSigma_m;
float noiseLevel;
Distribution::Normal<float> dNextStep;
Distribution::Normal<float> dNextStepStair;
public:
/** ctor with the walker to follow */
SyntheticSteps(SyntheticWalker* walker) {
SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.35, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) :
//stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m),
noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) {
walker->addListener(this);
dX.setSeed(1);
@@ -64,10 +77,16 @@ public:
// AccelerometerData acc(x,y,z);
// stepPattern.add(Timestamp::fromMS(i), acc);
// }
stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));
stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4));
stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5));
stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0));
}
@@ -79,18 +98,20 @@ public:
protected:
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {
(void) curPos;
const float nextStepAt = (lastStepAtDistance + stepSize_m);
const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw());
const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair);
const float nextStepAt = lastStepAtDistance + distAdd;
// 1st, start with random noise on the accelerometer
const float x = dX.draw();
const float y = dY.draw();
const float z = dZ.draw();
const AccelerometerData base(0, 4, 9.7);
const AccelerometerData noise(x, y, z);
AccelerometerData acc = base + noise;
const AccelerometerData aBase(0, 4, 9.7);
const AccelerometerData aNoise(x, y, z);
AccelerometerData acc = aBase + aNoise * noiseLevel;
// is it time to inject a "step" into the accelerometer data?
if (walkedDistance > nextStepAt) {
@@ -105,7 +126,7 @@ protected:
refStepPattern = Timestamp::fromMS(0);
} else {
const AccelerometerData step = stepPattern.get(curPatPos);
acc = base + noise*2.5f + step;
acc = aBase + (aNoise * noiseLevel) + step;
}
}

57
synthetic/SyntheticTurns.h
View File

@@ -7,7 +7,7 @@
#include "../sensors/imu/GyroscopeData.h"
#include "../geo/Heading.h"
#include "../math/distribution/Normal.h"
#include "../math/Distributions.h"
/**
* simulates acceleromter and gyroscope data
@@ -39,15 +39,23 @@ private:
Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
//float headingDrift_rad;
//float headingSigma_rad;
float noiseLevel;
Distribution::Normal<float> dHeadErr;
std::vector<Listener*> listeners;
public:
/** ctor with the walker to follow */
SyntheticTurns(SyntheticWalker* walker) {
SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) :
//headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad),
noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) {
walker->addListener(this);
dAccX.setSeed(1);
dAccY.setSeed(3);
@@ -55,6 +63,7 @@ public:
dGyroX.setSeed(7);
dGyroY.setSeed(9);
dGyroZ.setSeed(11);
}
/** attach a listener to this provider */
@@ -68,7 +77,7 @@ protected:
Heading desiredHead = Heading(0);
Heading curHead = Heading(0);
Point3 lastPos = Point3(NAN, NAN, NAN);
float change;
double change = 0;
inline float clamp(const float val, const float min, const float max) {
if (val < min) {return min;}
@@ -76,7 +85,7 @@ protected:
return val;
}
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {
// time sine last onWalk();
if (lastTs.isZero()) {lastTs = walkedTime; return;}
@@ -86,36 +95,42 @@ protected:
if (lastPos.x != lastPos.x) {
lastPos = curPos;
} else {
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();
lastPos = curPos;
}
// difference between current-heading and desired-heading
const float diffRad = Heading::getSignedDiff(curHead, desiredHead);
const double maxChange = dMaxChange.draw();
const double diffRad = Heading::getSignedDiff(curHead, desiredHead);
//change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange);
change = clamp(diffRad / 25, -maxChange, +maxChange);
// slowly change the current heading to match the desired one
const float maxChange = dMaxChange.draw();
const float toChange = clamp(diffRad, -maxChange, +maxChange);
//if (change < toChange) {change += toChange*0.01;}
if (change > toChange) {change *= 0.93;}
if (change < toChange) {change += dChange.draw()/10000;}
//if (change > toChange) {change -= dChange.draw();}
// // slowly change the current heading to match the desired one
// //const double maxChange = dMaxChange.draw();
// //const double toChange = clamp(diffRad, -maxChange, +maxChange);
// const double toChange = diffRad;
// //if (change < toChange) {change += toChange*0.01;}
// if (change > toChange) {change *= 0.93;}
// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?!
// if (change < toChange) {change += (toChange-change) * 0.07;}
// //if (change > toChange) {change -= dChange.draw();}
curHead += change;
// convert to gyro's radians-per-second
const float radPerSec = change * 1000 / deltaTs.ms();;
const double radPerSec = change * 1000 / deltaTs.ms();;
const float accX = 0.00 + dAccX.draw();
const float accY = 0.00 + dAccY.draw();
const float accZ = 9.81 + dAccZ.draw();
const float accX = 0.00 + dAccX.draw() * (noiseLevel);
const float accY = 0.00 + dAccY.draw() * (noiseLevel);
const float accZ = 9.81 + dAccZ.draw() * (noiseLevel);
AccelerometerData acc(accX, accY, accZ);
const float gyroX = dGyroX.draw();
const float gyroY = dGyroY.draw();
const float gyroZ = dGyroZ.draw() + radPerSec;
const float gyroX = dGyroX.draw() * (noiseLevel);
const float gyroY = dGyroY.draw() * (noiseLevel);
const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec;
GyroscopeData gyro(gyroX, gyroY, gyroZ);
for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}

15
synthetic/SyntheticWalker.h
View File

@@ -8,9 +8,14 @@ class SyntheticWalker {
public:
enum class Type {
FLOOR,
NON_FLOOR,
};
class Listener {
public:
virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0;
virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos, const Type type) = 0;
};
private:
@@ -44,6 +49,10 @@ public:
this->listeners.push_back(l);
}
bool done() {
return path.doneAtDistance(this->walkedDistance);
}
/** increment the walk */
Point3 tick(const Timestamp timePassed) {
@@ -55,11 +64,13 @@ public:
// get the current position along the path
const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance);
const bool isPlainPart = path.isPlain(this->walkedDistance);
const Type type = (isPlainPart) ? (Type::FLOOR) : (Type::NON_FLOOR);
Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() );
// inform listener
for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);}
for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath, type);}
return curPosOnPath;

296
tests/math/filter/TestButter.cpp
View File

@@ -23,234 +23,234 @@
TEST(Butterworth, offlineSinus) {
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
int size = 1100; //Fs
double* input = new double[size];
double* output = new double[size];
int size = 1100; //Fs
double* input = new double[size];
double* output = new double[size];
// 17.5hz sin signal with random noise [-0.1, 0.1]
for( int i=0; i < size; ++i ){
input[i] = sin(0.1 * i) + noise(gen);
}
// 17.5hz sin signal with random noise [-0.1, 0.1]
for( int i=0; i < size; ++i ){
input[i] = sin(0.1 * i) + noise(gen);
}
//butterworth
Filter::ButterworthLP<double> butter(size,20,5);
butter.stepInitialization(0);
butter.filter(input, output, size, 0, true);
//butterworth
Filter::ButterworthLP<double> butter(size,20,5);
butter.stepInitialization(0);
butter.filter(input, output, size, 0, true);
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
for(int i=0; i < size-1; ++i){
for(int i=0; i < size-1; ++i){
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesOutput.getStroke().getColor().setHexStr("#00FF00");
plot.add(&linesInput);
plot.add(&linesOutput);
plot.add(&linesInput);
plot.add(&linesOutput);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
sleep(10);
sleep(10);
}
TEST(Butterworth, onlineSinus) {
int size = 1100; //Fs
double* input = new double[size];
double* output = new double[size];
int size = 1100; //Fs
double* input = new double[size];
double* output = new double[size];
Filter::ButterworthLP<double> butter(size,20,5);
butter.stepInitialization(0);
Filter::ButterworthLP<double> butter(size,20,5);
butter.stepInitialization(0);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
// 17.5hz sin signal with random noise [-0.1, 0.1]
for( int i=0; i < size; ++i ){
input[i] = sin(0.1 * i) + noise(gen);
// 17.5hz sin signal with random noise [-0.1, 0.1]
for( int i=0; i < size; ++i ){
input[i] = sin(0.1 * i) + noise(gen);
output[i] = butter.process(input[i]);
}
output[i] = butter.process(input[i]);
}
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
for(int i=0; i < size-1; ++i){
for(int i=0; i < size-1; ++i){
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesOutput.getStroke().getColor().setHexStr("#00FF00");
plot.add(&linesInput);
plot.add(&linesOutput);
plot.add(&linesInput);
plot.add(&linesOutput);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
sleep(1);
sleep(1);
}
TEST(Butterworth, offlineOctaveBaro) {
double* input = new double[100000];
double* output = new double[100000];
double* input = new double[100000];
double* output = new double[100000];
Interpolator<int, double> interp;
Interpolator<int, double> interp;
//read file
std::string line;
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
std::ifstream infile(filename);
//read file
std::string line;
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
std::ifstream infile(filename);
int counter = 0;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
int counter = 0;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (iss >> ts >> value) {
while (iss >> ts >> value) {
interp.add(ts, value);
interp.add(ts, value);
while(interp.getMaxKey() > counter*20 ){
double interpValue = interp.get(counter*20);
while(interp.getMaxKey() > counter*20 ){
double interpValue = interp.get(counter*20);
input[counter] = interpValue;
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
++counter;
}
input[counter] = interpValue;
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
++counter;
}
//std::cout << ts << " " << value << " r" << std::endl;
//std::cout << ts << " " << value << " r" << std::endl;
}
}
}
}
Filter::ButterworthLP<double> butter(50,0.2,2);
butter.filter(input, output, counter, 938.15, true);
Filter::ButterworthLP<double> butter(50,0.2,2);
butter.filter(input, output, counter, 938.15, true);
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
for(int i=0; i < counter-1; ++i){
for(int i=0; i < counter-1; ++i){
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesOutput.getStroke().getColor().setHexStr("#00FF00");
plot.add(&linesInput);
plot.add(&linesOutput);
plot.add(&linesInput);
plot.add(&linesOutput);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
sleep(1);
sleep(1);
}
TEST(Butterworth, onlineOctaveBaro) {
std::vector<double> input;
std::vector<double> output;
std::vector<double> input;
std::vector<double> output;
Interpolator<int, double> interp;
Interpolator<int, double> interp;
Filter::ButterworthLP<double> butter(50,0.02,2);
butter.stepInitialization(938.15);
Filter::ButterworthLP<double> butter(50,0.02,2);
butter.stepInitialization(938.15);
//read file
std::string line;
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
std::ifstream infile(filename);
//read file
std::string line;
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
std::ifstream infile(filename);
int counter = 1;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
int counter = 1;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (iss >> ts >> value) {
while (iss >> ts >> value) {
interp.add(ts, value);
interp.add(ts, value);
while(interp.getMaxKey() > counter*20 ){
double interpValue = interp.get(counter*20);
while(interp.getMaxKey() > counter*20 ){
double interpValue = interp.get(counter*20);
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
input.push_back(interpValue);
input.push_back(interpValue);
output.push_back(butter.process(interpValue));
output.push_back(butter.process(interpValue));
++counter;
}
++counter;
}
//std::cout << ts << " " << value << " r" << std::endl;
//std::cout << ts << " " << value << " r" << std::endl;
}
}
}
}
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines linesInput;
K::GnuplotPlotElementLines linesOutput;
for(int i=0; i < input.size()-1; ++i){
for(int i=0; i < input.size()-1; ++i){
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 input_p1(i, input[i]);
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
K::GnuplotPoint2 output_p1(i, output[i]);
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesInput.addSegment(input_p1, input_p2);
linesOutput.addSegment(output_p1, output_p2);
}
linesOutput.getStroke().getColor().setHexStr("#00FF00");
plot.add(&linesInput);
plot.add(&linesOutput);
plot.add(&linesInput);
plot.add(&linesOutput);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
sleep(1);
sleep(1);
}

101
tests/navMesh/TestNavMeshBenchmark.cpp Normal file
View File

@@ -0,0 +1,101 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../navMesh/NavMeshFactory.h"
#include "../../navMesh/walk/NavMeshSub.h"
using namespace NM;
TEST(NavMeshBenchmark, benchDraw) {
Floorplan::IndoorMap map;
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
// circle (many triangles)
int i = 0;
for (float f = 0; f < M_PI*2; f += 0.1) {
const float x = std::cos(f) * 10;
const float y = std::sin(f) * 10;
outline.poly.points.push_back(Point2(x,y));
++i;
}
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMeshSettings set;
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
fac.build(&map);
ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5);
ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5);
ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5);
ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5);
ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5);
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
ASSERT_EQ(45, nm.getNumTriangles());
NavMeshRandom<NM::NavMeshTriangle> rnd = nm.getRandom();
for (int i = 0; i < 5000*1000; ++i) {
NavMeshLocation<NM::NavMeshTriangle> loc = rnd.draw();
}
}
TEST(NavMeshBenchmark, benchSubRegion) {
Floorplan::IndoorMap map;
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
// circle (many triangles)
int i = 0;
for (float f = 0; f < M_PI*2; f += 0.1) {
const float x = std::cos(f) * 10;
const float y = std::sin(f) * 10;
outline.poly.points.push_back(Point2(x,y));
++i;
}
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMeshSettings set;
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
fac.build(&map);
ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5);
ASSERT_NEAR(-10, nm.getBBox().getMin().y, 0.5);
ASSERT_NEAR( 0, nm.getBBox().getMin().z, 0.5);
ASSERT_NEAR(+10, nm.getBBox().getMax().x, 0.5);
ASSERT_NEAR(+10, nm.getBBox().getMax().y, 0.5);
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
ASSERT_EQ(45, nm.getNumTriangles());
std::minstd_rand gen(1337);
std::uniform_real_distribution<float> dist(0, M_PI*2);
for (int i = 0; i < 50000; ++i) {
const float f = dist(gen);
const float x = std::cos(f) * 9;
const float y = std::sin(f) * 9;
NavMeshLocation<NM::NavMeshTriangle> loc = nm.getLocation(Point3(x,y,0));
NavMeshSub<NM::NavMeshTriangle>(loc, 5);
}
}
#endif

40
tests/navMesh/TestNavMeshFactory.cpp Normal file
View File

@@ -0,0 +1,40 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../navMesh/NavMeshFactory.h"
using namespace NM;
TEST(NavMeshFactory, build1) {
Floorplan::IndoorMap map;
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
outline.poly.points.push_back(Point2(0,0));
outline.poly.points.push_back(Point2(10,0));
outline.poly.points.push_back(Point2(10,10));
outline.poly.points.push_back(Point2(0,10));
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMeshSettings set;
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm,set);
fac.build(&map);
ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5);
ASSERT_NEAR(0, nm.getBBox().getMin().y, 0.5);
ASSERT_NEAR(0, nm.getBBox().getMin().z, 0.5);
ASSERT_NEAR(10, nm.getBBox().getMax().x, 0.5);
ASSERT_NEAR(10, nm.getBBox().getMax().y, 0.5);
ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5);
ASSERT_EQ(2, nm.getNumTriangles());
// ASSERT_EQ(nm.getNeighbor(0,0), nm[1]);
// ASSERT_EQ(nm.getNeighbor(1,0), nm[0]);
}
#endif

87
tests/navMesh/TestNavMeshSub.cpp Normal file
View File

@@ -0,0 +1,87 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../navMesh/NavMeshFactory.h"
#include "../../navMesh/walk/NavMeshSub.h"
using namespace NM;
TEST(NavMeshSub, build1) {
Floorplan::IndoorMap map;
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
outline.poly.points.push_back(Point2(0,0));
outline.poly.points.push_back(Point2(10,0));
outline.poly.points.push_back(Point2(10,10));
outline.poly.points.push_back(Point2(0,10));
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
NavMeshSettings set;
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
fac.build(&map);
nm.getLocation(Point3(1,1,0));
nm.getLocation(Point3(8,0.2,0));
nm.getLocation(Point3(0.2,8,0));
nm.getLocation(Point3(4.5,4.5,0));
}
TEST(NavMeshSub, draw) {
Floorplan::IndoorMap map;
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
outline.outdoor = false;
outline.method = Floorplan::OutlineMethod::ADD;
// circle (many triangles)
int i = 0;
for (float f = 0; f < M_PI*2; f += 0.1) {
const float x = std::cos(f) * 10;
const float y = std::sin(f) * 10;
outline.poly.points.push_back(Point2(x,y));
++i;
}
Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
remove.outdoor = false;
remove.method = Floorplan::OutlineMethod::REMOVE;
remove.poly.points.push_back(Point2(-2,-2));
remove.poly.points.push_back(Point2(+2,-2));
remove.poly.points.push_back(Point2(+2,+2));
remove.poly.points.push_back(Point2(-2,+2));
NavMeshSettings set;
NavMesh<NM::NavMeshTriangle> nm;
NavMeshFactory<NM::NavMeshTriangle> fac(&nm, set);
fac.build(&map);
NavMeshRandom<NM::NavMeshTriangle> rnd = nm.getRandom();
for (int i = 0; i < 1000; ++i) {
NavMeshLocation<NM::NavMeshTriangle> loc = rnd.draw();
ASSERT_TRUE(loc.tria->contains(loc.pos));
NavMeshSub<NM::NavMeshTriangle> sub2(loc, 5);
NavMeshRandom<NM::NavMeshTriangle> rnd2 = sub2.getRandom();
for (int j = 0; j < 100; ++j) {
NavMeshLocation<NM::NavMeshTriangle> loc2 = rnd2.draw();
ASSERT_TRUE(loc2.tria->contains(loc2.pos));
ASSERT_TRUE(sub2.contains(loc2.pos));
}
}
}
#endif

36
tests/navMesh/TestNavMeshTriangle.cpp Normal file
View File

@@ -0,0 +1,36 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../navMesh/NavMeshTriangle.h"
using namespace NM;
TEST(NavMeshTriangle, contains) {
NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
ASSERT_TRUE(t1.contains(Point3(0,0,0)));
ASSERT_TRUE(t1.contains(Point3(1,0,0)));
ASSERT_TRUE(t1.contains(Point3(0,1,0)));
ASSERT_TRUE(t1.contains(Point3(0.5,0.5,0)));
ASSERT_FALSE(t1.contains(Point3(0.501,0.5,0)));
ASSERT_FALSE(t1.contains(Point3(0.5,0.501,0)));
ASSERT_FALSE(t1.contains(Point3(1,1,0)));
}
TEST(NavMeshTriangle, area) {
NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1);
ASSERT_NEAR(0.5, t1.getArea(), 0.0001);
NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9), 1);
ASSERT_NEAR(0.5, t2.getArea(), 0.0001);
}
#endif

227
tests/sensors/imu/TestMotionDetection.cpp
View File

@@ -1,4 +1,4 @@
#ifdef WITH_TESTS
#ifdef TODO_________WITH_TESTS
#include "../../Tests.h"
@@ -9,191 +9,190 @@
/** visualize the motionAxis */
TEST(MotionDetection, motionAxis) {
MotionDetection md;
MotionDetection md;
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
//Walking with smartphone straight and always parallel to motion axis
//std::string filename = getDataFile("motion/straight_potrait.csv");
//Walking with smartphone straight and always parallel to motion axis
//std::string filename = getDataFile("motion/straight_potrait.csv");
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
std::string filename = getDataFile("motion/straight_landscape_left.csv");
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
std::string filename = getDataFile("motion/straight_landscape_left.csv");
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait.csv");
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait.csv");
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_landscape.csv");
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_landscape.csv");
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
//std::string filename = getDataFile("motion/rounds_pocket.csv");
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
//std::string filename = getDataFile("motion/rounds_pocket.csv");
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv");
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv");
Offline::FileReader fr(filename);
K::Gnuplot gp;
K::GnuplotPlot plot;
K::Gnuplot gp;
K::GnuplotPlot plot;
gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
gp << "set xrange[-1:1]\n set yrange[-1:1]\n";
Eigen::Vector2f curVec;
float motionAxisAngleRad;
Timestamp ts;
Timestamp lastTs;
Eigen::Vector2f curVec;
float motionAxisAngleRad;
Timestamp ts;
Timestamp lastTs;
//calc motion axis
//calc motion axis
for (const Offline::Entry& e : fr.getEntries()) {
ts = Timestamp::fromMS(e.ts);
ts = Timestamp::fromMS(e.ts);
if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
curVec = md.getCurrentMotionAxis();
motionAxisAngleRad = md.getMotionChangeInRad();
}
md.addGravity(ts, fr.getGravity()[e.idx].data);
curVec = md.getCurrentMotionAxis();
motionAxisAngleRad = md.getMotionChangeInRad();
}
// start with the first available timestamp
if (lastTs.isZero()) {lastTs = ts;}
// start with the first available timestamp
if (lastTs.isZero()) {lastTs = ts;}
if(ts - lastTs > Timestamp::fromMS(500)) {
if(ts - lastTs > Timestamp::fromMS(500)) {
lastTs = ts;
lastTs = ts;
K::GnuplotPoint2 raw_p1(0, 0);
K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
K::GnuplotPlotElementLines motionLines;
motionLines.addSegment(raw_p1, raw_p2);
plot.add(&motionLines);
K::GnuplotPoint2 raw_p1(0, 0);
K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0));
K::GnuplotPlotElementLines motionLines;
motionLines.addSegment(raw_p1, raw_p2);
plot.add(&motionLines);
gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n";
gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n";
gp.draw(plot);
gp.flush();
//usleep(5000*33);
}
}
gp.draw(plot);
gp.flush();
//usleep(5000*33);
}
}
//was passiert bei grenzwerten. 90° oder sowas.
//wie stabil ist die motion axis eigentlich?
//erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
//wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
//was passiert bei grenzwerten. 90° oder sowas.
//wie stabil ist die motion axis eigentlich?
//erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen?
//wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading?
}
/** comparing motionAngle and turnAngle */
TEST(MotionDetection, motionAngle) {
MotionDetection md;
PoseDetection pd;
TurnDetection td(&pd);
MotionDetection md;
TurnDetection td;
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
//Walking with smartphone straight and always parallel to motion axis
std::string filename = getDataFile("motion/straight_potrait.csv");
//Walking with smartphone straight and always parallel to motion axis
std::string filename = getDataFile("motion/straight_potrait.csv");
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
//std::string filename = getDataFile("motion/straight_landscape_left.csv");
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
//straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right.
//std::string filename = getDataFile("motion/straight_landscape_left.csv");
//std::string filename = getDataFile("motion/straight_landscape_right.csv");
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
//straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase
//std::string filename = getDataFile("motion/straight_inturn_landscape.csv");
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait.csv");
//rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait.csv");
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_landscape.csv");
//round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns.
//std::string filename = getDataFile("motion/rounds_landscape.csv");
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
//round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns.
//std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv");
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
//std::string filename = getDataFile("motion/rounds_pocket.csv");
//rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me)
//std::string filename = getDataFile("motion/rounds_pocket.csv");
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv");
//table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm.
//std::string filename = getDataFile("motion/table_flat.csv");
Offline::FileReader fr(filename);
Timestamp ts;
Timestamp ts;
//save for later plotting
std::vector<float> delta_motionAngles;
std::vector<float> delta_turnAngles;
//save for later plotting
std::vector<float> delta_motionAngles;
std::vector<float> delta_turnAngles;
//calc motion axis
//calc motion axis
for (const Offline::Entry& e : fr.getEntries()) {
ts = Timestamp::fromMS(e.ts);
ts = Timestamp::fromMS(e.ts);
if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
delta_motionAngles.push_back(md.getMotionChangeInRad());
md.addGravity(ts, fr.getGravity()[e.idx].data);
delta_motionAngles.push_back(md.getMotionChangeInRad());
} else if (e.type == Offline::Sensor::ACC) {
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
pd.addAccelerometer(ts, _acc.data);
td.addAccelerometer(ts, _acc.data);
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
}
delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data));
}
}
}
//draw motion
static K::Gnuplot gpMotion;
K::GnuplotPlot plotMotion;
K::GnuplotPlotElementLines motionLines;
//draw motion
static K::Gnuplot gpMotion;
K::GnuplotPlot plotMotion;
K::GnuplotPlotElementLines motionLines;
for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
for(int i = 0; i < delta_motionAngles.size() - 1; ++i){
K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
motionLines.addSegment(raw_p1, raw_p2);
K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]);
K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]);
motionLines.addSegment(raw_p1, raw_p2);
}
}
gpMotion << "set title 'Motion Detection'\n";
plotMotion.add(&motionLines);
gpMotion.draw(plotMotion);
gpMotion.flush();
gpMotion << "set title 'Motion Detection'\n";
plotMotion.add(&motionLines);
gpMotion.draw(plotMotion);
gpMotion.flush();
//draw rotation
static K::Gnuplot gpTurn;
K::GnuplotPlot plotTurn;
K::GnuplotPlotElementLines turnLines;
//draw rotation
static K::Gnuplot gpTurn;
K::GnuplotPlot plotTurn;
K::GnuplotPlotElementLines turnLines;
for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
for(int i = 0; i < delta_turnAngles.size() - 1; ++i){
K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
turnLines.addSegment(raw_p1, raw_p2);
}
K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]);
K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]);
turnLines.addSegment(raw_p1, raw_p2);
}
gpTurn << "set title 'Turn Detection'\n";
plotTurn.add(&turnLines);
gpTurn.draw(plotTurn);
gpTurn.flush();
gpTurn << "set title 'Turn Detection'\n";
plotTurn.add(&turnLines);
gpTurn.draw(plotTurn);
gpTurn.flush();
sleep(1);
sleep(1);
}

40
tests/sensors/imu/TestTurnDetection.cpp
View File

@@ -6,34 +6,33 @@
TEST(TurnDetection, rotationMatrix) {
Vector3 dst(0, 0, 1);
Vector3 src(1, 1, 0);
src = src.normalized();
Vector3 dst(0, 0, 1);
Vector3 src(1, 1, 0); src = src.normalized();
// get a matrix that rotates "src" into "dst"
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
Vector3 res = rot * src;
Vector3 res = rot * src;
ASSERT_NEAR(dst.x, res.x, 0.01);
ASSERT_NEAR(dst.y, res.y, 0.01);
ASSERT_NEAR(dst.z, res.z, 0.01);
ASSERT_NEAR(dst.x, res.x, 0.01);
ASSERT_NEAR(dst.y, res.y, 0.01);
ASSERT_NEAR(dst.z, res.z, 0.01);
}
TEST(TurnDetection, gyroRotate) {
Vector3 zAxis(0, 0, 1);
Vector3 acc(0, 7.0, 7.0);
Vector3 zAxis(0, 0, 1);
Vector3 acc(0, 7.0, 7.0);
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);
Vector3 gyro(0, 60, 60);
Vector3 gyro(0, 60, 60);
Vector3 gyro2(0, 0, 84);
Vector3 gyro2(0, 0, 84);
Vector3 gyro3 = rot * gyro;
Vector3 gyro3 = rot * gyro;
ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);
@@ -42,11 +41,10 @@ TEST(TurnDetection, gyroRotate) {
TEST(TurnDetection, xx) {
Vector3 dst(0, 0, 1);
Vector3 src(0.0, 2.9, -10.0);
src = src.normalized(); // sample accelerometer readings
Vector3 dst(0, 0, 1);
Vector3 src(0.0, 2.9, -10.0); src = src.normalized(); // sample accelerometer readings
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);
// Eigen::Vector3f x; x << 1, 0, 0;
// Eigen::Vector3f z = src.normalized();
@@ -57,14 +55,14 @@ TEST(TurnDetection, xx) {
// rot.row(1) = y;
// rot.row(2) = z;
Vector3 res = rot * src;
Vector3 res = rot * src;
// ASSERT_NEAR(dst(0), res(0), 0.01);
// ASSERT_NEAR(dst(1), res(1), 0.01);
// ASSERT_NEAR(dst(2), res(2), 0.01);
Vector3 gyro(0, 10, 30);
Vector3 gyro(0, 10, 30);
Vector3 gyro2 = rot * gyro;
Vector3 gyro2 = rot * gyro;
int i = 0; (void) i;
}

163
tests/sensors/pressure/TestBarometer.cpp
View File

@@ -1,4 +1,4 @@
#ifdef WITH_TESTS
#ifdef TODO_______WITH_TESTS
#include "../../Tests.h"
@@ -115,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) {
}
sleep(1);
sleep(1);
// tendence must be clear and smaller than the sigma
@@ -124,120 +124,119 @@ TEST(Barometer, LIVE_tendence2) {
}
TEST(Barometer, Activity) {
ActivityButterPressure act;
ActivityButterPressure act;
//read file
std::string line;
//read file
std::string line;
std::string filename = getDataFile("barometer/baro1.dat");
std::ifstream infile(filename);
std::ifstream infile(filename);
std::vector<ActivityButterPressure::History> actHist;
std::vector<ActivityButterPressure::History> rawHist;
std::vector<ActivityButterPressure::History> actHist;
std::vector<ActivityButterPressure::History> rawHist;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (iss >> ts >> value) {
act.add(Timestamp::fromMS(ts), BarometerData(value));
Activity currentAct = act.get();
rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData((int)currentAct)));
}
}
while (iss >> ts >> value) {
ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value));
rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value)));
actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct)));
}
}
K::Gnuplot gp;
K::Gnuplot gpRaw;
K::GnuplotPlot plot;
K::GnuplotPlot plotRaw;
K::GnuplotPlotElementLines rawLines;
K::GnuplotPlotElementLines resultLines;
K::Gnuplot gp;
K::Gnuplot gpRaw;
K::GnuplotPlot plot;
K::GnuplotPlot plotRaw;
K::GnuplotPlotElementLines rawLines;
K::GnuplotPlotElementLines resultLines;
for(int i=0; i < actHist.size()-1; ++i){
for(int i=0; i < actHist.size()-1; ++i){
//raw
K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
//raw
K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa);
K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa);
rawLines.addSegment(raw_p1, raw_p2);
rawLines.addSegment(raw_p1, raw_p2);
//results
K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
//results
K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa);
K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa);
resultLines.addSegment(input_p1, input_p2);
}
resultLines.addSegment(input_p1, input_p2);
}
plotRaw.add(&rawLines);
plot.add(&resultLines);
plotRaw.add(&rawLines);
plot.add(&resultLines);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
gpRaw.draw(plotRaw);
gpRaw.flush();
gpRaw.draw(plotRaw);
gpRaw.flush();
sleep(5);
sleep(5);
}
TEST(Barometer, ActivityPercent) {
ActivityButterPressurePercent act;
ActivityButterPressurePercent act;
//read file
std::string line;
std::string filename = getDataFile("barometer/baro1.dat");
std::ifstream infile(filename);
//read file
std::string line;
std::string filename = getDataFile("barometer/baro1.dat");
std::ifstream infile(filename);
std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
std::vector<double> rawHist;
std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
std::vector<double> rawHist;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (std::getline(infile, line))
{
std::istringstream iss(line);
int ts;
double value;
while (iss >> ts >> value) {
ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
rawHist.push_back(value);
actHist.push_back(activity);
}
}
while (iss >> ts >> value) {
ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value));
rawHist.push_back(value);
actHist.push_back(activity);
}
}
K::Gnuplot gp;
K::Gnuplot gpRaw;
K::GnuplotPlot plot;
K::GnuplotPlot plotRaw;
K::GnuplotPlotElementLines rawLines;
K::GnuplotPlotElementLines resultLines;
K::Gnuplot gp;
K::Gnuplot gpRaw;
K::GnuplotPlot plot;
K::GnuplotPlot plotRaw;
K::GnuplotPlotElementLines rawLines;
K::GnuplotPlotElementLines resultLines;
for(int i=0; i < actHist.size()-1; ++i){
for(int i=0; i < actHist.size()-1; ++i){
K::GnuplotPoint2 raw_p1(i, rawHist[i]);
K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
K::GnuplotPoint2 raw_p1(i, rawHist[i]);
K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]);
rawLines.addSegment(raw_p1, raw_p2);
rawLines.addSegment(raw_p1, raw_p2);
K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown);
K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown);
resultLines.addSegment(input_p1, input_p2);
}
resultLines.addSegment(input_p1, input_p2);
}
plotRaw.add(&rawLines);
plot.add(&resultLines);
plotRaw.add(&rawLines);
plot.add(&resultLines);
gp.draw(plot);
gp.flush();
gp.draw(plot);
gp.flush();
gpRaw.draw(plotRaw);
gpRaw.flush();
gpRaw.draw(plotRaw);
gpRaw.flush();
sleep(5);
sleep(5);
}