298 lines
7.4 KiB
C++
298 lines
7.4 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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
|