frank
1c2081d406
adjusted grid factory adjusted nav mesh factory minoor changes/fixes new helper classes refactoring
288 lines
7.1 KiB
C++
288 lines
7.1 KiB
C++
#ifndef GRID_FACTORY_HELPER_H
|
|
#define GRID_FACTORY_HELPER_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 HelperPoly {
|
|
|
|
BBox2 bbox_cm;
|
|
std::vector<Point2> points_cm;
|
|
|
|
/** empty ctor */
|
|
HelperPoly() {
|
|
;
|
|
}
|
|
|
|
/** ctor from floorplan-polygon */
|
|
HelperPoly(const Floorplan::FloorOutlinePolygon& poly) {
|
|
for (Point2 p : poly.poly.points) { add(p * 100); }
|
|
}
|
|
|
|
/** ctor from floorplan-quad */
|
|
HelperPoly(const Floorplan::Quad3& quad) {
|
|
add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
|
|
}
|
|
|
|
/** ctor from floorplan-polygon */
|
|
HelperPoly(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());
|
|
}
|
|
|
|
|
|
void shrink(float cm) {
|
|
Point2 center;
|
|
for (const Point2 pt : points_cm) {center += pt;}
|
|
center /= points_cm.size();
|
|
for (Point2& pt : points_cm) {
|
|
Point2 dir = pt - center;
|
|
float len = dir.length();
|
|
dir = dir.normalized();
|
|
pt = center + dir * (len-cm);
|
|
}
|
|
}
|
|
|
|
/** 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);
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
template <typename T> class Helper {
|
|
|
|
private:
|
|
|
|
Grid<T>& grid;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Helper(Grid<T>& grid) : grid(grid) {
|
|
|
|
}
|
|
|
|
/** connect the given node to all its neighbors (x,y) */
|
|
void connectToNeighbors(T& n1) {
|
|
|
|
const int gs_cm = grid.getGridSize_cm();
|
|
|
|
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
|
|
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
|
|
|
|
// skip the node itself
|
|
if (x == 0 && y == 0) {continue;}
|
|
|
|
// try to find a matching neighbor
|
|
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm);
|
|
const T* n2 = grid.getNodePtrFor(gp);
|
|
if (!n2) {continue;}
|
|
|
|
// connect
|
|
if (n1.hasNeighbor(n2->getIdx())) {continue;}
|
|
grid.connectUniDir(n1, *n2);
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/** connect the given node to all its neighbors )x,y,z) */
|
|
void connectToNeighborsXYZ(T& n1) {
|
|
|
|
const int gs_cm = grid.getGridSize_cm();
|
|
|
|
for (int z = -gs_cm; z <= +gs_cm; z += gs_cm) {
|
|
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
|
|
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
|
|
|
|
// skip the node itself
|
|
if (x == 0 && y == 0 && z == 0) {continue;}
|
|
|
|
// try to find a matching neighbor
|
|
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm + z);
|
|
const T* n2 = grid.getNodePtrFor(gp);
|
|
if (!n2) {continue;}
|
|
|
|
// connect
|
|
if (n1.hasNeighbor(n2->getIdx())) {continue;}
|
|
grid.connectUniDir(n1, *n2);
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
int gridSize() const {
|
|
return grid.getGridSize_cm();;
|
|
}
|
|
|
|
float align(const float val) {
|
|
const float gridSize_cm = gridSize();
|
|
return std::round(val/gridSize_cm) * gridSize_cm;
|
|
}
|
|
|
|
float alignF(const float val) {
|
|
const float gridSize_cm = gridSize();
|
|
return std::floor(val/gridSize_cm) * gridSize_cm;
|
|
}
|
|
|
|
float alignC(const float val) {
|
|
const float gridSize_cm = gridSize();
|
|
return std::ceil(val/gridSize_cm) * gridSize_cm;
|
|
}
|
|
|
|
Point3 align(const Point3 p) {
|
|
return Point3( align(p.x), align(p.y), (p.z) ); // TODO: align z or not?
|
|
}
|
|
|
|
float min(float a, float b, float c, float d) {
|
|
if (a < b && a < c && a < d) {return a;}
|
|
if (b < a && b < c && b < d) {return b;}
|
|
if (c < a && c < b && c < d) {return c;}
|
|
if (d < a && d < b && d < c) {return d;}
|
|
}
|
|
float max(float a, float b, float c, float d) {
|
|
if (a > b && a > c && a > d) {return a;}
|
|
if (b > a && b > c && b > d) {return b;}
|
|
if (c > a && c > b && c > d) {return c;}
|
|
if (d > a && d > b && d > c) {return d;}
|
|
}
|
|
|
|
void clamp(float& val, const int min, const int max) {
|
|
while (val < min) {val += gridSize();}
|
|
while (val > max) {val -= gridSize();}
|
|
}
|
|
|
|
|
|
void limit(Floorplan::Quad3& q2, const Floorplan::Quad3& q1) {
|
|
const float x1 = min(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
|
|
const float x2 = max(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
|
|
const float y1 = min(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
|
|
const float y2 = max(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
|
|
const float z1 = min(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
|
|
const float z2 = max(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
|
|
// clamp(q2.p1.x, x1, x2); clamp(q2.p2.x, x1, x2); clamp(q2.p3.x, x1, x2); clamp(q2.p4.x, x1, x2);
|
|
// clamp(q2.p1.y, y1, y2); clamp(q2.p2.y, y1, y2); clamp(q2.p3.y, y1, y2); clamp(q2.p4.y, y1, y2);
|
|
clamp(q2.p1.z, z1, z2); clamp(q2.p2.z, z1, z2); clamp(q2.p3.z, z1, z2); clamp(q2.p4.z, z1, z2);
|
|
}
|
|
|
|
Floorplan::Quad3 align(const Floorplan::Quad3& q) {
|
|
Floorplan::Quad3 q2 = Floorplan::Quad3(align(q.p1), align(q.p2), align(q.p3), align(q.p4));
|
|
//limit(q2, q);
|
|
return q2;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static float dot(const Point2 a, const Point2 b) {
|
|
return a.x * b.x + a.y * b.y;
|
|
}
|
|
|
|
static float area(const Point2 a, const Point2 b, const Point2 c) {
|
|
//Point2 ab = b-a;
|
|
//Point2 ac = c-a;
|
|
//return 0.5f * std::sqrt(dot(ab,ab)*dot(ac,ac) - dot(ab,ac)*dot(ab,ac));
|
|
Point2 p1 = b-a;
|
|
Point2 p2 = c-a;
|
|
return std::abs(p1.x*p2.y - p2.x*p1.y) * 0.5;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // GRID_FACTORY_HELPER_H
|