From 686151b51122805869700818b508047e8fbaa43d Mon Sep 17 00:00:00 2001 From: FrankE Date: Wed, 13 Sep 2017 08:08:00 +0200 Subject: [PATCH] worked on 2D/3D raytracing adjusted BVH improved 2D/3D BVH new bounding volumes new test cases renamed some test-cases for grouping reasons made GPC header-only using slight adjustments --- CMakeLists.txt | 4 +- geo/BBox2.h | 22 +- geo/BBox3.h | 7 + geo/Circle2.h | 304 ++++++++++++++++++ geo/Point2.h | 5 + geo/Point3.h | 6 +- geo/Ray2.h | 29 ++ geo/Ray3.h | 6 +- geo/Sphere3.h | 8 +- geo/volume/BVH.h | 102 +++++- geo/volume/BVHDebug.h | 186 +++++++++-- geo/volume/BoundingVolumeAABB2.h | 36 +++ ...dingVolumeAABB.h => BoundingVolumeAABB3.h} | 0 geo/volume/BoundingVolumeBox.h | 4 - geo/volume/BoundingVolumeCircle2.h | 38 +++ geo/volume/BoundingVolumeHierarchy.h | 4 - ...VolumeSphere.h => BoundingVolumeSphere3.h} | 18 +- lib/gpc/gpc.cpp.h | 156 ++++----- main.cpp | 3 +- math/DrawList.h | 2 +- tests/geo/TestAngle.cpp | 10 +- tests/geo/{TestBBox.cpp => TestBBox2.cpp} | 8 +- tests/geo/TestBBox3.cpp | 18 ++ tests/geo/TestBVH2.cpp | 176 ++++++++++ tests/geo/{TestBVH.cpp => TestBVH3.cpp} | 10 +- tests/geo/TestCircle2.cpp | 85 +++++ tests/geo/TestHeading.cpp | 8 +- tests/geo/TestLength.cpp | 4 +- tests/geo/TestPoint2.cpp | 23 ++ tests/geo/{TestPoint.cpp => TestPoint3.cpp} | 2 +- tests/geo/TestSphere3.cpp | 12 +- tests/geo/TestTriangle3.cpp | 2 +- tests/ray/TestRayTrace2.cpp | 37 +++ tests/ray/TestRayTrace3.cpp | 3 +- wifi/estimate/ray2d/DataMap2.h | 1 + wifi/estimate/ray2d/Ray2.h | 24 -- wifi/estimate/ray2d/WiFiRayTrace2D.h | 144 ++++++--- wifi/estimate/ray3/WifiRayTrace3D.h | 3 +- 38 files changed, 1257 insertions(+), 253 deletions(-) create mode 100644 geo/Circle2.h create mode 100644 geo/Ray2.h create mode 100644 geo/volume/BoundingVolumeAABB2.h rename geo/volume/{BoundingVolumeAABB.h => BoundingVolumeAABB3.h} (100%) delete mode 100644 geo/volume/BoundingVolumeBox.h create mode 100644 geo/volume/BoundingVolumeCircle2.h delete mode 100644 geo/volume/BoundingVolumeHierarchy.h rename geo/volume/{BoundingVolumeSphere.h => BoundingVolumeSphere3.h} (59%) rename tests/geo/{TestBBox.cpp => TestBBox2.cpp} (91%) create mode 100644 tests/geo/TestBBox3.cpp create mode 100644 tests/geo/TestBVH2.cpp rename tests/geo/{TestBVH.cpp => TestBVH3.cpp} (93%) create mode 100644 tests/geo/TestCircle2.cpp create mode 100644 tests/geo/TestPoint2.cpp rename tests/geo/{TestPoint.cpp => TestPoint3.cpp} (93%) create mode 100644 tests/ray/TestRayTrace2.cpp delete mode 100644 wifi/estimate/ray2d/Ray2.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e0e7862..f214537 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,8 +75,8 @@ ADD_DEFINITIONS( -fstack-protector-all -g3 -# -O0 - -O2 + -O0 + #-O2 -march=native -DWITH_TESTS diff --git a/geo/BBox2.h b/geo/BBox2.h index 1503f01..950136d 100644 --- a/geo/BBox2.h +++ b/geo/BBox2.h @@ -1,8 +1,9 @@ -#ifndef BBOX2_H -#define BBOX2_H +#ifndef GEO_BBOX2_H +#define GEO_BBOX2_H #include "Point2.h" #include "Line2.h" +#include class BBox2 { @@ -105,6 +106,21 @@ public: p2 += Point2(val, val); // increase maximum } + /** grow the bbox by the amount given for each dimension */ + void growRel(const float val) { + const Point2 center = (p1+p2)/2; + p1 += (p1-center)*val; // decrease minimum + p2 += (p2-center)*val; // increase maximum + } + + + /** combine two bboxes */ + static BBox2 join(const BBox2& bb1, const BBox2& bb2) { + const Point2 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y) ); + const Point2 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y) ); + return BBox2(min, max); + } + }; -#endif // BBOX2_H +#endif // GEO_BBOX2_H diff --git a/geo/BBox3.h b/geo/BBox3.h index ed81f6c..6218b2f 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -103,6 +103,13 @@ public: return true; } + /** combine two bboxes */ + static BBox3 join(const BBox3& bb1, const BBox3& bb2) { + const Point3 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y), std::min(bb1.p1.z, bb2.p1.z) ); + const Point3 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y), std::max(bb1.p2.z, bb2.p2.z) ); + return BBox3(min,max); + } + }; #endif // BBOX3_H diff --git a/geo/Circle2.h b/geo/Circle2.h new file mode 100644 index 0000000..49eccc2 --- /dev/null +++ b/geo/Circle2.h @@ -0,0 +1,304 @@ +#ifndef CIRCLE2_H +#define CIRCLE2_H + +#include + +#include "Point2.h" +#include "Ray2.h" + +#include "../Assertions.h" + +//#include +//#include +//#include +//#include + +struct Circle2 { + + Point2 center; + + float radius; + +public: + + /** empty ctor */ + Circle2() : center(), radius(0) {;} + + /** ctor */ + Circle2(const Point2 center, const float radius) : center(center), radius(radius) {;} + + + /** does this circle contain the given point? */ + bool contains(const Point2 p) const { + return center.getDistance(p) <= radius; + } + + /** does this circle contain the given point? */ + bool containsAll(const std::vector& pts) const { + for (const Point2& p : pts) { + if (!contains(p)) {return false;} + } + return true; + } + + /** get a point on the circle for the given radians */ + Point2 getPointAt(const float rad) const { + return center + Point2(std::cos(rad), std::sin(rad)) * radius; + } + + + /** does this circle contain the given circle? */ + bool contains(const Circle2 c) const { + return (center.getDistance(c.center)+c.radius) <= radius; + } + + /** does this circle intersect with the given ray? */ + bool intersects(const Ray2 ray) const { + + // https://math.stackexchange.com/questions/311921/get-location-of-vector-circle-intersection + const float a = ray.dir.x*ray.dir.x + ray.dir.y*ray.dir.y; + const float b = 2 * ray.dir.x * (ray.start.x-center.x) + 2 * ray.dir.y * (ray.start.y - center.y); + const float c = (ray.start.x-center.x) * (ray.start.x-center.x) + (ray.start.y - center.y)*(ray.start.y - center.y) - radius*radius; + const float discr = b*b - 4*a*c; + + return discr >= 0; + + } + + + /** configure this sphere to contain the given point-set */ + void adjustToPointSet(const std::vector& lst) { + + //adjustToPointSetAPX(lst); + adjustToPointSetExact(lst); + + // validate + for (const Point2& p : lst) { + Assert::isTrue(this->contains(p), "calculated circle seems incorrect"); + } + + } + + /** combine two circles into a new one containing both */ + static Circle2 join(const Circle2& a, const Circle2& b) { + + // if one already contains the other, just return it as-is + if (a.contains(b)) {return a;} + if (b.contains(a)) {return b;} + + // create both maximum ends + const Point2 out1 = a.center + (a.center-b.center).normalized() * a.radius; + const Point2 out2 = b.center + (b.center-a.center).normalized() * b.radius; + + // center is within both ends, so is the radius + Point2 newCen = (out1 + out2) / 2; + float newRad = out1.getDistance(out2) / 2 * 1.0001; // slightly larger to prevent rounding issues + Circle2 res(newCen, newRad); + + // check + Assert::isTrue(res.contains(a), "sphere joining error. base-spheres are not contained."); + Assert::isTrue(res.contains(b), "sphere joining error. base-spheres are not contained."); + + return res; + + } + + float getArea() const { + return M_PI * (radius*radius); + } + +private: + + /* + void show(std::vector pts, const Point2 P, const Point2 Q, const Point2 R = Point2(NAN, NAN)) { + + static K::Gnuplot gp; + + K::GnuplotPlot plot; + K::GnuplotPlotElementPoints gPoints; plot.add(&gPoints); gPoints.setColor(K::GnuplotColor::fromHexStr("#0000ff")); gPoints.setPointSize(1); + K::GnuplotPlotElementLines gLines; plot.add(&gLines); + + for (const Point2 p : pts) { + K::GnuplotPoint2 p2(p.x, p.y); + gPoints.add(p2); + } + + K::GnuplotPoint2 gP(P.x, P.y); + K::GnuplotPoint2 gQ(Q.x, Q.y); + gLines.addSegment(gP, gQ); + + K::GnuplotPoint2 gR(R.x, R.y); + //gLines.addSegment(gP, gR); + gLines.addSegment(gQ, gR); + + for (float f = 0; f < M_PI*2; f += 0.1) { + Point2 p = center + Point2(std::cos(f), std::sin(f)) * radius; + K::GnuplotPoint2 gp (p.x, p.y); + gLines.add(gp); + } + + gp << "set size ratio -1\n"; + gp.draw(plot); + gp.flush(); + + int i = 0; (void) i; + + } + */ + + + // Graphic Gems 2 - Jon Rokne + void adjustToPointSetExact(const std::vector& _lst) { + + if (_lst.size() == 2) { + this->center = (_lst[0] + _lst[1]) / 2; + this->radius = _lst[0].getDistance(_lst[1]) / 2 * 1.0001f; + return; + } + + std::vector lst = _lst; + + // find point P having min(p.y) + // NOTE: seems like the original work uses another coordinate system. so we search for max(p.y) instead! + auto compMinY = [] (const Point2 p1, const Point2 p2) {return p1.y < p2.y;}; + auto it1 = std::max_element(lst.begin(), lst.end(), compMinY); + Point2 P = *it1; + lst.erase(it1); + + // find a point Q such that the angle of the line segment + // PQ with the x axis is minimal + auto compMinAngleX = [&] (const Point2 p1, const Point2 p2) { + + const Point2 PQ1 = p1 - P; + const Point2 PQ2 = p2 - P; + + const float angle1 = dot(PQ1.normalized(), Point2(0,1)); + const float angle2 = dot(PQ2.normalized(), Point2(0,1)); + + return std::acos(angle1) < std::acos(angle2); + + }; + auto it2 = std::min_element(lst.begin(), lst.end(), compMinAngleX); + Point2 Q = *it2; + lst.erase(it2); + + // get the angle abc which is the angle at "b" + auto angle = [] (const Point2 a, const Point2 b, const Point2 c) { + const Point2 ba = a - b; + const Point2 bc = c - b; + return std::acos(dot(ba.normalized(), bc.normalized())); + }; + + // TODO: how many loops? + for (size_t xx = 0; xx < lst.size()*10; ++xx) { + + auto compMinAnglePRQ = [P,Q,angle] (const Point2 p1, const Point2 p2) { + return std::abs(angle(P,p1,Q)) < std::abs(angle(P,p2,Q)); + }; + + auto it3 = std::min_element(lst.begin(), lst.end(), compMinAnglePRQ); + + Point2 R = *it3; + lst.erase(it3); + + const float anglePRQ = angle(P,R,Q); + const float angleRPQ = angle(R,P,Q); + const float anglePQR = angle(P,Q,R); + + //check for case 1 (angle PRQ is obtuse), the circle is determined + //by two points, P and Q. radius = |(P-Q)/2|, center = (P+Q)/2 + if (anglePRQ > M_PI/2) { + + this->radius = P.getDistance(Q) / 2 * 1.001f; + this->center = (P+Q)/2; + //if (!containsAll(_lst)) {show(_lst, P, Q, R);} + return; + + } + + if (angleRPQ > M_PI/2) { + lst.push_back(P); + P = R; + continue; + } + + if (anglePQR > M_PI/2) { + lst.push_back(Q); + Q = R; + continue; + } + + const Point2 mPQ = (P+Q)/2; + const Point2 mQR = (Q+R)/2; + + const float numer = -(-mPQ.y * R.y + mPQ.y * Q.y + mQR.y * R.y - mQR.y * Q.y - mPQ.x * R.x + mPQ.x * Q.x + mQR.x * R.x - mQR.x * Q.x); + const float denom = (-Q.x * R.y + P.x * R.y - P.x * Q.y + Q.y * R.x - P.y * R.x + P.y * Q.x); + const float t = numer / denom; + + const float cx = -t * (Q.y - P.y) + mPQ.x; + const float cy = t * (Q.x - P.x) + mPQ.y; + this->center = Point2(cx, cy); + this->radius = this->center.getDistance(P) * 1.001f; + + //if (!containsAll(_lst)) {show(_lst, P, Q, R);} + + return; + + } + + throw Exception("should not happen"); + + } + + /* + void adjustToPointSetRefine(const std::vector& lst) { + + float bestArea = 99999999; + + for (size_t i = 0; i < lst.size(); ++i) { + for (size_t j = 0; j < lst.size(); ++j) { + if (i == j) {continue;} + + const Point2 center = (lst[i] + lst[j]) / 3; + const float radius = lst[i].getDistance(lst[j]); + const Circle2 test(center, radius); + + if (test.containsAll(lst)) { + if (test.getArea() < bestArea) { + bestArea = test.getArea(); + this->radius = test.radius; + this->center = test.center; + } + } + } + } + + } +*/ + void adjustToPointSetAPX(const std::vector& lst) { + + // NOT OPTIMAL but fast + + // calculate the point set's center + Point2 sum(0,0); + for (const Point2 p : lst) {sum += p;} + const Point2 center = sum / lst.size(); + + // calculate the sphere's radius (maximum distance from center + float radius = 0; + for (const Point2 p : lst) { + const float dist = center.getDistance(p); + if (dist > radius) {radius = dist;} + } + + this->radius = radius; + this->center = center; + + + + + } + +}; + +#endif // CIRCLE2_H diff --git a/geo/Point2.h b/geo/Point2.h index f28e23d..a7d594a 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -68,6 +68,11 @@ struct Point2 { }; +inline float dot(const Point2 p1, const Point2 p2) { + return (p1.x*p2.x) + (p1.y*p2.y); +} + + inline void swap(Point2& p1, Point2& p2) { std::swap(p1.x, p2.x); std::swap(p1.y, p2.y); diff --git a/geo/Point3.h b/geo/Point3.h index f436d38..2269191 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -1,5 +1,5 @@ -#ifndef POINT3_H -#define POINT3_H +#ifndef GEO_POINT3_H +#define GEO_POINT3_H #include "../Assertions.h" #include @@ -128,4 +128,4 @@ inline Point3 cross(const Point3 a, const Point3 b) { ); } -#endif // POINT3_H +#endif // GEO_POINT3_H diff --git a/geo/Ray2.h b/geo/Ray2.h new file mode 100644 index 0000000..a53e2cc --- /dev/null +++ b/geo/Ray2.h @@ -0,0 +1,29 @@ +#ifndef GEO_RAY2_H +#define GEO_RAY2_H + + +#include "Point2.h" + +struct Ray2 { + + /** starting point */ + Point2 start; + + /** ray's direction */ + Point2 dir; + +public: + + /** empty */ + Ray2() : start(), dir() { + ; + } + + /** ctor */ + Ray2(Point2 start, Point2 dir) : start(start), dir(dir) { + ; + } + +}; + +#endif // GEO_RAY2_H diff --git a/geo/Ray3.h b/geo/Ray3.h index eb23012..3adab2a 100644 --- a/geo/Ray3.h +++ b/geo/Ray3.h @@ -1,5 +1,5 @@ -#ifndef RAY3_H -#define RAY3_H +#ifndef GEO_RAY3_H +#define GEO_RAY3_H #include "Point3.h" @@ -25,4 +25,4 @@ public: }; -#endif // RAY3_H +#endif // GEO_RAY3_H diff --git a/geo/Sphere3.h b/geo/Sphere3.h index ed2bfda..9b71b38 100644 --- a/geo/Sphere3.h +++ b/geo/Sphere3.h @@ -1,5 +1,5 @@ -#ifndef SPHERE3_H -#define SPHERE3_H +#ifndef GEO_SPHERE3_H +#define GEO_SPHERE3_H #include "Point3.h" #include "Ray3.h" @@ -172,7 +172,7 @@ public: // center is within both ends, so is the radius Point3 newCen = (out1 + out2) / 2; - float newRad = out1.getDistance(out2) / 2 * 1.001; // slightly larger to prevent rounding issues + float newRad = out1.getDistance(out2) / 2 * 1.0001; // slightly larger to prevent rounding issues Sphere3 res(newCen, newRad); // check @@ -185,4 +185,4 @@ public: }; -#endif // SPHERE3_H +#endif // GEO_SPHERE3_H diff --git a/geo/volume/BVH.h b/geo/volume/BVH.h index 180f3d9..36d3963 100644 --- a/geo/volume/BVH.h +++ b/geo/volume/BVH.h @@ -4,29 +4,36 @@ #include #include +#include "../Ray2.h" +#include "../Ray3.h" + #include "BoundingVolume.h" -#include "BoundingVolumeAABB.h" -#include "BoundingVolumeSphere.h" + +#include "BoundingVolumeAABB2.h" +#include "BoundingVolumeCircle2.h" + +#include "BoundingVolumeAABB3.h" +#include "BoundingVolumeSphere3.h" - -template class BVH { +template class BVH { protected: /** one node within the tree */ struct BVHNode { - bool isLeaf = true; + bool isLeaf; + bool check; Volume boundingVolume; std::vector childNodes; - BVHNode(bool isLeaf = false) : isLeaf(isLeaf) {;} + BVHNode(bool isLeaf = false, bool check = true) : isLeaf(isLeaf), check(check) {;} }; /** one leaf within the tree */ struct BVHLeaf : public BVHNode { Element element; - BVHLeaf(const Element& e) : BVHNode(true), element(e) {;} + BVHLeaf(const Element& e, const bool check) : BVHNode(true, check), element(e) {;} }; /** the tree's root */ @@ -40,10 +47,10 @@ public: } /** add a new volume to the tree */ - void add(const Element& element) { + void add(const Element& element, const bool leafCheck = true) { // create a new leaf for this element - BVHLeaf* leaf = new BVHLeaf(element); + BVHLeaf* leaf = new BVHLeaf(element, leafCheck); // get the element's boundin volume leaf->boundingVolume = getBoundingVolume(element); @@ -63,17 +70,17 @@ public: return max; } - void getHits(const Ray3 ray, std::function func) const { - //int tests = 0; int leafs = 0; + + void getHits(const Ray& ray, const std::function& func) const { getHits(ray, &root, func); - //std::cout << tests << " " << leafs << std::endl; } - void getHits(const Ray3 ray, const BVHNode* node, std::function func) const { + // this one has to be as fast as possible! + static void getHits(const Ray& ray, const BVHNode* node, const std::function& func) { for (const BVHNode* sub : node->childNodes) { - if (sub->boundingVolume.intersects(ray)) { + if (!sub->check || sub->boundingVolume.intersects(ray)) { if (sub->isLeaf) { - BVHLeaf* leaf = (BVHLeaf*)(sub); // TODO: cast + const BVHLeaf* leaf = static_cast(sub); func(leaf->element); } else { getHits(ray, sub, func); @@ -82,8 +89,50 @@ public: } } + /** get the tree's depth */ + int getDepth() const { + return getDepth(&root, 1); + } + + private: + /** call the given function for each leaf within the given subtree */ + void forEachLeaf(const BVHNode* n, std::function func) const { + if (n->isLeaf) { + func(n); + } else { + for (BVHNode* child : n->childNodes) { + forEachLeaf(child, func); + } + } + } + + /** determine/approximate a new bounding volume around n1+n2 */ + Volume getVolAround(const BVHNode* n1, const BVHNode* n2) const { + //return getVolAroundExact(n1, n2); + return getVolAroundAPX(n1, n2); + } + + /** determine the bounding-volume around n1 and n2 by (slowly) calculating a new, exact volume based on all leaf-elements */ + Volume getVolAroundExact(const BVHNode* n1, const BVHNode* n2) const { + std::vector verts; + auto onLeaf = [&] (const BVHNode* n) { + BVHLeaf* leaf = (BVHLeaf*) n; + std::vector subVerts = Wrapper::getVertices(leaf->element); + verts.insert(verts.end(), subVerts.begin(), subVerts.end()); + }; + forEachLeaf(n1, onLeaf); + forEachLeaf(n2, onLeaf); + return Volume::fromVertices(verts); + } + + /** approximate the bounding-volume around n1 and n2 by (quickly) joining their current volumes. the result might be unnecessarily large */ + Volume getVolAroundAPX(const BVHNode* n1, const BVHNode* n2) const { + return Volume::join(n1->boundingVolume, n2->boundingVolume); + } + + bool combineBest() { // nothing to do? @@ -104,7 +153,7 @@ private: BVHNode* n1 = root.childNodes[i]; BVHNode* n2 = root.childNodes[j]; - const Volume newVol = Volume::join(n1->boundingVolume, n2->boundingVolume); + const Volume newVol = getVolAround(n1,n2); const float newVolSize = newVol.getVolumeSize(); if (newVolSize < best.volSize) { best.vol = newVol; @@ -226,13 +275,32 @@ private: } + int getDepth(const BVHNode* node, const int cur) const { + if (node->isLeaf) { + return cur; + } else { + int res = cur; + for (const BVHNode* sub : node->childNodes) { + const int subDepth = getDepth(sub, cur+1); + if (subDepth > res) {res = subDepth;} + } + return res; + } + } + /** get a bounding-volume for the given element */ Volume getBoundingVolume(const Element& element) { - const std::vector verts = Wrapper::getVertices(element); + const std::vector verts = Wrapper::getVertices(element); return Volume::fromVertices(verts); } +}; +template class BVH3 : public BVH { + +}; + +template class BVH2 : public BVH { }; diff --git a/geo/volume/BVHDebug.h b/geo/volume/BVHDebug.h index e247462..466ca54 100644 --- a/geo/volume/BVHDebug.h +++ b/geo/volume/BVHDebug.h @@ -2,39 +2,52 @@ #define BVHDEBUG_H #include "BVH.h" + + +#include + #include #include #include #include -#include + +#include +#include +#include +#include + #include "../BBox3.h" #include -/** adds some debug helpers to the BVH */ -template class BVHDebug : public BVH { +///** adds some debug helpers to the BVH */ +//template class BVHDebug : public BVH { + + +//}; + +template class BVH3Debug : public BVH { + + using BVHNode = typename BVH::BVHNode; + using BVHLeaf = typename BVH::BVHLeaf; - using BVHNode = typename BVH::BVHNode; - using BVHLeaf = typename BVH::BVHLeaf; public: -// std::vecto colors { -// "#888888", "#888800", "#008888", "#880088", "#ee0000", "#00ee00", "#0000ee" -// }; - void show(int maxPts = 1500, bool showLeafs = true) { std::stringstream out; static K::Gnuplot gp; K::GnuplotSplot plot; - K::GnuplotSplotElementColorPoints pVol; plot.add(&pVol); //pVol.setColor(K::GnuplotColor::fromRGB(128,128,128)); - K::GnuplotSplotElementPoints pElemPoints; plot.add(&pElemPoints); pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255)); - K::GnuplotSplotElementLines pElemLines; plot.add(&pElemLines); pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255)); - const int depth = recurse(maxPts, showLeafs, 0, &this->root, pVol, pElemPoints, pElemLines); + K::GnuplotSplotElementColorPoints pVol; plot.add(&pVol); //pVol.setColor(K::GnuplotColor::fromRGB(128,128,128)); + K::GnuplotSplotElementPoints pElemPoints; plot.add(&pElemPoints); pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255)); + K::GnuplotSplotElementLines pElemLines; plot.add(&pElemLines); pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255)); - plot.getAxisCB().setRange(0, depth); + const int maxDepth = this->getDepth(); + recurse(maxPts, showLeafs, 0, &this->root, pVol, pElemPoints, pElemLines); + + plot.getAxisCB().setRange(0, maxDepth); gp << "set view equal xyz\n"; gp.draw(plot); @@ -44,6 +57,31 @@ public: private: + Point3 getRandomPoint(BoundingVolumeSphere3 sphere) { + static std::minstd_rand gen; + std::uniform_real_distribution dist(-1, +1); + Point3 dir = Point3(dist(gen), dist(gen), dist(gen)).normalized() * sphere.radius; + return sphere.center + dir; + } + + void addLines(const Element& elem, K::GnuplotSplotElementLines& elemLines) { + + std::vector pts = Wrapper::getDebugLines(elem); + + for (size_t i = 0; i< pts.size(); i+=2) { + + const Point3 p1 = pts[i+0]; + const Point3 p2 = pts[i+1]; + + K::GnuplotPoint3 gp1(p1.x, p1.y, p1.z); + K::GnuplotPoint3 gp2(p2.x, p2.y, p2.z); + + elemLines.addSegment(gp1, gp2); + + } + + } + int recurse(int maxPts, bool showLeafs, int curDepth, const BVHNode* node, K::GnuplotSplotElementColorPoints& vol, K::GnuplotSplotElementPoints& pElemPoints, K::GnuplotSplotElementLines& elemLines) { int resDepth = curDepth; @@ -76,24 +114,53 @@ private: } - Point3 getRandomPoint(BoundingVolumeSphere sphere) { - static std::minstd_rand gen; - std::uniform_real_distribution dist(-1, +1); - Point3 dir = Point3(dist(gen), dist(gen), dist(gen)).normalized() * sphere.radius; - return sphere.center + dir; +}; + +template class BVH2Debug : public BVH { + + using BVHNode = typename BVH::BVHNode; + using BVHLeaf = typename BVH::BVHLeaf; + +public: + + void show(int maxPts = 1500, bool showLeafs = true) { + + std::stringstream out; + + static K::Gnuplot gp; + K::GnuplotPlot plot; + + + K::GnuplotPlotElementColorLines pVolLines; plot.add(&pVolLines); + K::GnuplotPlotElementPoints pElemPoints; plot.add(&pElemPoints); pElemPoints.setColor(K::GnuplotColor::fromRGB(0,0,255)); + K::GnuplotPlotElementLines pElemLines; plot.add(&pElemLines); pElemLines.getStroke().setColor(K::GnuplotColor::fromRGB(0,0,255)); + + const int maxDepth = this->getDepth(); + recurse(maxDepth, showLeafs, 0, &this->root, plot, pVolLines, pElemPoints, pElemLines); + + plot.getObjects().reOrderByZIndex(); + plot.getAxisCB().setRange(0, maxDepth); + + gp << "set size ratio -1\n"; + gp.draw(plot); + gp.flush(); + } - void addLines(const Element& elem, K::GnuplotSplotElementLines& elemLines) { +private: - std::vector pts = Wrapper::getDebugLines(elem); + + void addLines(const Element& elem, K::GnuplotPlotElementLines& elemLines) { + + std::vector pts = Wrapper::getDebugLines(elem); for (size_t i = 0; i< pts.size(); i+=2) { - const Point3 p1 = pts[i+0]; - const Point3 p2 = pts[i+1]; + const Point2 p1 = pts[i+0]; + const Point2 p2 = pts[i+1]; - K::GnuplotPoint3 gp1(p1.x, p1.y, p1.z); - K::GnuplotPoint3 gp2(p2.x, p2.y, p2.z); + K::GnuplotPoint2 gp1(p1.x, p1.y); + K::GnuplotPoint2 gp2(p2.x, p2.y); elemLines.addSegment(gp1, gp2); @@ -101,6 +168,75 @@ private: } + std::vector colors = { + "#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088", + "#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088", + "#888800", "#444400", "#008888", "#004444", "#880088", "#440044", "#ee0000", "#880000", "#00ee00", "#008800", "#0000ee", "#000088" + }; + + void showVolume(const BoundingVolumeCircle2& circle, int maxDepth, int curDepth, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines) { + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + for (int i = 0; i < 20; ++i) { + const float f = M_PI*2 * i / 19; + const Point2 p = circle.getPointAt(f); + poly->add(K::GnuplotCoordinate2(p.x, p.y, K::GnuplotCoordinateSystem::FIRST)); + poly->getFill().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth])); + poly->getFill().setStyle(K::GnuplotFillStyle::SOLID); + poly->setZIndex(curDepth); + } + plot.getObjects().add(poly); + } + + void showVolume(const BoundingVolumeAABB2& _aabb, int maxDepth, int curDepth, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines) { + BBox2 bbox2 = _aabb; + bbox2.grow( (10-curDepth) / 100.0f ); +// pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMin().y), curDepth); +// pVolLines.add(K::GnuplotPoint2(bbox2.getMax().x, bbox2.getMin().y), curDepth); +// pVolLines.add(K::GnuplotPoint2(bbox2.getMax().x, bbox2.getMax().y), curDepth); +// pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMax().y), curDepth); +// pVolLines.add(K::GnuplotPoint2(bbox2.getMin().x, bbox2.getMin().y), curDepth); +// pVolLines.splitFace(); + K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(); + poly->getStroke().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth])); + //poly->getFill().setColor(K::GnuplotColor::fromHexStr(colors[maxDepth-curDepth])); + //poly->getFill().setStyle(K::GnuplotFillStyle::SOLID); + poly->add(K::GnuplotCoordinate2(bbox2.getMin().x, bbox2.getMin().y, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate2(bbox2.getMax().x, bbox2.getMin().y, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate2(bbox2.getMax().x, bbox2.getMax().y, K::GnuplotCoordinateSystem::FIRST)); + poly->add(K::GnuplotCoordinate2(bbox2.getMin().x, bbox2.getMax().y, K::GnuplotCoordinateSystem::FIRST)); + poly->close(); + poly->setZIndex(curDepth); + plot.getObjects().add(poly); + } + + int recurse(int maxDepth, bool showLeafs, int curDepth, const BVHNode* node, K::GnuplotPlot& plot, K::GnuplotPlotElementColorLines& pVolLines, K::GnuplotPlotElementPoints& pElemPoints, K::GnuplotPlotElementLines& elemLines) { + + int resDepth = curDepth; + + for (BVHNode* sub : node->childNodes) { + resDepth = recurse(maxDepth, showLeafs, curDepth+1, sub, plot, pVolLines, pElemPoints, elemLines); + } + + if (!node->isLeaf || showLeafs) { + if (node != &this->root) { + //const int numPts = maxPts / (curDepth+1); + showVolume(node->boundingVolume, maxDepth, curDepth, plot, pVolLines); + } + } + + if (node->isLeaf) { + BVHLeaf* leaf = (BVHLeaf*) node; + std::vector verts = Wrapper::getVertices(leaf->element); + for (const Point2 p : verts) { + pElemPoints.add(K::GnuplotPoint2(p.x, p.y)); + } + addLines(leaf->element, elemLines); + } + + return resDepth; + + } + }; #endif // BVHDEBUG_H diff --git a/geo/volume/BoundingVolumeAABB2.h b/geo/volume/BoundingVolumeAABB2.h new file mode 100644 index 0000000..1abd746 --- /dev/null +++ b/geo/volume/BoundingVolumeAABB2.h @@ -0,0 +1,36 @@ +#ifndef BOUNDINGVOLUMEAABB2_H +#define BOUNDINGVOLUMEAABB2_H + +#include + +#include "BoundingVolume.h" +#include "../BBox2.h" + +class BoundingVolumeAABB2 : public BBox2 { + +public: + + BoundingVolumeAABB2() {;} + + BoundingVolumeAABB2(const BBox2& bb) : BBox2(bb) {;} + + float getVolumeSize() const { + const float dx = getMax().x - getMin().x; + const float dy = getMax().y - getMin().y; + return (dx*dy); + } + + /** construct a volume around the given point-set */ + static BoundingVolumeAABB2 fromVertices(const std::vector& verts) { + BoundingVolumeAABB2 bvs; + for (const Point2 p : verts) {bvs.add(p);} + return bvs; + } + + static BoundingVolumeAABB2 join(const BoundingVolumeAABB2 a, const BoundingVolumeAABB2 b) { + return BoundingVolumeAABB2(BBox2::join(a, b)); + } + +}; + +#endif // BOUNDINGVOLUMEAABB2_H diff --git a/geo/volume/BoundingVolumeAABB.h b/geo/volume/BoundingVolumeAABB3.h similarity index 100% rename from geo/volume/BoundingVolumeAABB.h rename to geo/volume/BoundingVolumeAABB3.h diff --git a/geo/volume/BoundingVolumeBox.h b/geo/volume/BoundingVolumeBox.h deleted file mode 100644 index 60ec5cd..0000000 --- a/geo/volume/BoundingVolumeBox.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef BOUNDINGVOLUMEBOX_H -#define BOUNDINGVOLUMEBOX_H - -#endif // BOUNDINGVOLUMEBOX_H diff --git a/geo/volume/BoundingVolumeCircle2.h b/geo/volume/BoundingVolumeCircle2.h new file mode 100644 index 0000000..672398e --- /dev/null +++ b/geo/volume/BoundingVolumeCircle2.h @@ -0,0 +1,38 @@ +#ifndef BOUDINGVOLUMECIRCLE2_H +#define BOUDINGVOLUMECIRCLE2_H + +#include + +#include "BoundingVolume.h" +#include "../Circle2.h" + +class BoundingVolumeCircle2 : public Circle2 { + +public: + + BoundingVolumeCircle2() {;} + + BoundingVolumeCircle2(const Circle2& c) : Circle2(c) {;} + + float getVolumeSize() const { + return M_PI * (radius*radius); + } + + bool intersects(const Ray2 ray) const { + return Circle2::intersects(ray); + } + + /** construct a volume around the given point-set */ + static BoundingVolumeCircle2 fromVertices(const std::vector& verts) { + BoundingVolumeCircle2 bvs; + bvs.adjustToPointSet(verts); + return bvs; + } + + static BoundingVolumeCircle2 join(const BoundingVolumeCircle2 a, const BoundingVolumeCircle2 b) { + return BoundingVolumeCircle2(Circle2::join(a, b)); + } + +}; + +#endif // BOUDINGVOLUMECIRCLE2_H diff --git a/geo/volume/BoundingVolumeHierarchy.h b/geo/volume/BoundingVolumeHierarchy.h deleted file mode 100644 index 3682e2e..0000000 --- a/geo/volume/BoundingVolumeHierarchy.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef BOUNDINGVOLUMEHIERARCHY_H -#define BOUNDINGVOLUMEHIERARCHY_H - -#endif // BOUNDINGVOLUMEHIERARCHY_H diff --git a/geo/volume/BoundingVolumeSphere.h b/geo/volume/BoundingVolumeSphere3.h similarity index 59% rename from geo/volume/BoundingVolumeSphere.h rename to geo/volume/BoundingVolumeSphere3.h index 83a71f4..aac59f3 100644 --- a/geo/volume/BoundingVolumeSphere.h +++ b/geo/volume/BoundingVolumeSphere3.h @@ -5,13 +5,13 @@ #include "../Sphere3.h" #include "../Point3.h" -class BoundingVolumeSphere : public BoundingVolume, public Sphere3 { +class BoundingVolumeSphere3 : public BoundingVolume, public Sphere3 { public: - BoundingVolumeSphere() {;} + BoundingVolumeSphere3() {;} - BoundingVolumeSphere(const Sphere3& s) : Sphere3(s) {;} + BoundingVolumeSphere3(const Sphere3& s) : Sphere3(s) {;} float getVolumeSize() const { return (4.0f / 3.0f) * M_PI * (radius*radius*radius); @@ -27,25 +27,25 @@ public: /** does the volume intersect with the given volume? */ bool intersects(const BoundingVolume& other) const { - const BoundingVolumeSphere& sphere = (const BoundingVolumeSphere&) other; + const BoundingVolumeSphere3& sphere = (const BoundingVolumeSphere3&) other; return Sphere3::intersects(sphere); } /** does the volume contain the given volume? */ bool contains(const BoundingVolume& other) const { - const BoundingVolumeSphere& sphere = (const BoundingVolumeSphere&) other; + const BoundingVolumeSphere3& sphere = (const BoundingVolumeSphere3&) other; return Sphere3::contains(sphere); } /** construct a volume around the given point-set */ - static BoundingVolumeSphere fromVertices(const std::vector& verts) { - BoundingVolumeSphere bvs; + static BoundingVolumeSphere3 fromVertices(const std::vector& verts) { + BoundingVolumeSphere3 bvs; bvs.adjustToPointSet(verts); return bvs; } - static BoundingVolumeSphere join(const BoundingVolumeSphere a, const BoundingVolumeSphere b) { - return BoundingVolumeSphere(Sphere3::join(a, b)); + static BoundingVolumeSphere3 join(const BoundingVolumeSphere3 a, const BoundingVolumeSphere3 b) { + return BoundingVolumeSphere3(Sphere3::join(a, b)); } }; diff --git a/lib/gpc/gpc.cpp.h b/lib/gpc/gpc.cpp.h index 114d9d1..f317ab1 100755 --- a/lib/gpc/gpc.cpp.h +++ b/lib/gpc/gpc.cpp.h @@ -54,8 +54,8 @@ Copyright: (C) Advanced Interfaces Group, #define TRUE 1 #endif -#define LEFT 0 -#define RIGHT 1 +#define GPC_LEFT 0 +#define GPC_RIGHT 1 #define ABOVE 0 #define BELOW 1 @@ -504,8 +504,8 @@ static edge_node *build_lmt(lmt_node **lmt, sb_tree **sbtree, &(e[i + 1]) : NULL; e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; e[i].next_bound= NULL; - e[i].bside[CLIP]= (op == GPC_DIFF) ? RIGHT : LEFT; - e[i].bside[SUBJ]= LEFT; + e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT; + e[i].bside[SUBJ]= GPC_LEFT; } insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); } @@ -554,8 +554,8 @@ static edge_node *build_lmt(lmt_node **lmt, sb_tree **sbtree, &(e[i + 1]) : NULL; e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; e[i].next_bound= NULL; - e[i].bside[CLIP]= (op == GPC_DIFF) ? RIGHT : LEFT; - e[i].bside[SUBJ]= LEFT; + e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT; + e[i].bside[SUBJ]= GPC_LEFT; } insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); } @@ -736,7 +736,7 @@ static int count_contours(polygon_node *polygon) { /* Count the vertices in the current contour */ nv= 0; - for (v= polygon->proxy->v[LEFT]; v; v= v->next) + for (v= polygon->proxy->v[GPC_LEFT]; v; v= v->next) nv++; /* Record valid vertex counts in the active field */ @@ -748,7 +748,7 @@ static int count_contours(polygon_node *polygon) else { /* Invalid contour: just free the heap */ - for (v= polygon->proxy->v[LEFT]; v; v= nextv) + for (v= polygon->proxy->v[GPC_LEFT]; v; v= nextv) { nextv= v->next; FREE(v); @@ -770,10 +770,10 @@ static void add_left(polygon_node *p, double x, double y) nv->y= y; /* Add vertex nv to the left end of the polygon's vertex list */ - nv->next= p->proxy->v[LEFT]; + nv->next= p->proxy->v[GPC_LEFT]; - /* Update proxy->[LEFT] to point to nv */ - p->proxy->v[LEFT]= nv; + /* Update proxy->[GPC_LEFT] to point to nv */ + p->proxy->v[GPC_LEFT]= nv; } @@ -787,8 +787,8 @@ static void merge_left(polygon_node *p, polygon_node *q, polygon_node *list) if (p->proxy != q->proxy) { /* Assign p's vertex list to the left end of q's list */ - p->proxy->v[RIGHT]->next= q->proxy->v[LEFT]; - q->proxy->v[LEFT]= p->proxy->v[LEFT]; + p->proxy->v[GPC_RIGHT]->next= q->proxy->v[GPC_LEFT]; + q->proxy->v[GPC_LEFT]= p->proxy->v[GPC_LEFT]; /* Redirect any p->proxy references to q->proxy */ @@ -815,10 +815,10 @@ static void add_right(polygon_node *p, double x, double y) nv->next= NULL; /* Add vertex nv to the right end of the polygon's vertex list */ - p->proxy->v[RIGHT]->next= nv; + p->proxy->v[GPC_RIGHT]->next= nv; - /* Update proxy->v[RIGHT] to point to nv */ - p->proxy->v[RIGHT]= nv; + /* Update proxy->v[GPC_RIGHT] to point to nv */ + p->proxy->v[GPC_RIGHT]= nv; } @@ -832,8 +832,8 @@ static void merge_right(polygon_node *p, polygon_node *q, polygon_node *list) if (p->proxy != q->proxy) { /* Assign p's vertex list to the right end of q's list */ - q->proxy->v[RIGHT]->next= p->proxy->v[LEFT]; - q->proxy->v[RIGHT]= p->proxy->v[RIGHT]; + q->proxy->v[GPC_RIGHT]->next= p->proxy->v[GPC_LEFT]; + q->proxy->v[GPC_RIGHT]= p->proxy->v[GPC_RIGHT]; /* Redirect any p->proxy references to q->proxy */ for (target= p->proxy; list; list= list->next) @@ -869,9 +869,9 @@ static void add_local_min(polygon_node **p, edge_node *edge, (*p)->active= TRUE; (*p)->next= existing_min; - /* Make v[LEFT] and v[RIGHT] point to new vertex nv */ - (*p)->v[LEFT]= nv; - (*p)->v[RIGHT]= nv; + /* Make v[GPC_LEFT] and v[GPC_RIGHT] point to new vertex nv */ + (*p)->v[GPC_LEFT]= nv; + (*p)->v[GPC_RIGHT]= nv; /* Assign polygon p to the edge */ edge->outp[ABOVE]= *p; @@ -911,10 +911,10 @@ static void new_tristrip(polygon_node **tn, edge_node *edge, { MALLOC(*tn, sizeof(polygon_node), "tristrip node creation", polygon_node); (*tn)->next= NULL; - (*tn)->v[LEFT]= NULL; - (*tn)->v[RIGHT]= NULL; + (*tn)->v[GPC_LEFT]= NULL; + (*tn)->v[GPC_RIGHT]= NULL; (*tn)->active= 1; - add_vertex(&((*tn)->v[LEFT]), x, y); + add_vertex(&((*tn)->v[GPC_LEFT]), x, y); edge->outp[ABOVE]= *tn; } else @@ -1125,7 +1125,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, polygon_node *out_poly= NULL, *p, *q, *poly, *npoly, *cf= NULL; vertex_node *vtx, *nv; h_state horiz[2]; - int in[2], exists[2], parity[2]= {LEFT, LEFT}; + int in[2], exists[2], parity[2]= {GPC_LEFT, GPC_LEFT}; int c, v, contributing, search, scanbeam= 0, sbt_entries= 0; int vclass, bl, br, tl, tr; double *sbt= NULL, xb, px, yb, yt, dy, ix, iy; @@ -1178,7 +1178,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* Invert clip polygon for difference operation */ if (op == GPC_DIFF) - parity[CLIP]= RIGHT; + parity[CLIP]= GPC_RIGHT; local_min= lmt; @@ -1721,7 +1721,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, "vertex creation", gpc_vertex); v= result->contour[c].num_vertices - 1; - for (vtx= poly->proxy->v[LEFT]; vtx; vtx= nv) + for (vtx= poly->proxy->v[GPC_LEFT]; vtx; vtx= nv) { nv= vtx->next; result->contour[c].vertex[v].x= vtx->x; @@ -1741,7 +1741,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, npoly= poly->next; FREE(poly); } - } + } /* Tidy up */ reset_it(&it); @@ -1786,7 +1786,7 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, vertex_node *lt, *ltn, *rt, *rtn; h_state horiz[2]; vertex_type cft; - int in[2], exists[2], parity[2]= {LEFT, LEFT}; + int in[2], exists[2], parity[2]= {GPC_LEFT, GPC_LEFT}; int s, v, contributing, search, scanbeam= 0, sbt_entries= 0; int vclass, bl, br, tl, tr; double *sbt= NULL, xb, px, nx, yb, yt, dy, ix, iy; @@ -1831,7 +1831,7 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* Invert clip polygon for difference operation */ if (op == GPC_DIFF) - parity[CLIP]= RIGHT; + parity[CLIP]= GPC_RIGHT; local_min= lmt; @@ -1990,17 +1990,17 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, case ERI: edge->outp[ABOVE]= cf->outp[ABOVE]; if (xb != cf->xb) - VERTEX(edge, ABOVE, RIGHT, xb, yb); + VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); cf= NULL; break; case ELI: - VERTEX(edge, BELOW, LEFT, xb, yb); + VERTEX(edge, BELOW, GPC_LEFT, xb, yb); edge->outp[ABOVE]= NULL; cf= edge; break; case EMX: if (xb != cf->xb) - VERTEX(edge, BELOW, RIGHT, xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); edge->outp[ABOVE]= NULL; cf= NULL; break; @@ -2008,11 +2008,11 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, if (cft == LED) { if (cf->bot.y != yb) - VERTEX(cf, BELOW, LEFT, cf->xb, yb); + VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); new_tristrip(&tlist, cf, cf->xb, yb); } edge->outp[ABOVE]= cf->outp[ABOVE]; - VERTEX(edge, ABOVE, RIGHT, xb, yb); + VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); break; case ILI: new_tristrip(&tlist, edge, xb, yb); @@ -2023,33 +2023,33 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, if (cft == LED) { if (cf->bot.y != yb) - VERTEX(cf, BELOW, LEFT, cf->xb, yb); + VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); new_tristrip(&tlist, cf, cf->xb, yb); } - VERTEX(edge, BELOW, RIGHT, xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); edge->outp[ABOVE]= NULL; break; case IMX: - VERTEX(edge, BELOW, LEFT, xb, yb); + VERTEX(edge, BELOW, GPC_LEFT, xb, yb); edge->outp[ABOVE]= NULL; cft= IMX; break; case IMM: - VERTEX(edge, BELOW, LEFT, xb, yb); + VERTEX(edge, BELOW, GPC_LEFT, xb, yb); edge->outp[ABOVE]= cf->outp[ABOVE]; if (xb != cf->xb) - VERTEX(cf, ABOVE, RIGHT, xb, yb); + VERTEX(cf, ABOVE, GPC_RIGHT, xb, yb); cf= edge; break; case EMM: - VERTEX(edge, BELOW, RIGHT, xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); edge->outp[ABOVE]= NULL; new_tristrip(&tlist, edge, xb, yb); cf= edge; break; case LED: if (edge->bot.y == yb) - VERTEX(edge, BELOW, LEFT, xb, yb); + VERTEX(edge, BELOW, GPC_LEFT, xb, yb); edge->outp[ABOVE]= edge->outp[BELOW]; cf= edge; cft= LED; @@ -2060,21 +2060,21 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, { if (cf->bot.y == yb) { - VERTEX(edge, BELOW, RIGHT, xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); } else { if (edge->bot.y == yb) { - VERTEX(cf, BELOW, LEFT, cf->xb, yb); - VERTEX(edge, BELOW, RIGHT, xb, yb); + VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); } } } else { - VERTEX(edge, BELOW, RIGHT, xb, yb); - VERTEX(edge, ABOVE, RIGHT, xb, yb); + VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); + VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); } cf= NULL; break; @@ -2199,8 +2199,8 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, if (p) { P_EDGE(prev_edge, e0, ABOVE, px, iy); - VERTEX(prev_edge, ABOVE, LEFT, px, iy); - VERTEX(e0, ABOVE, RIGHT, ix, iy); + VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); + VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); e1->outp[ABOVE]= e0->outp[ABOVE]; e0->outp[ABOVE]= NULL; } @@ -2209,8 +2209,8 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, if (q) { N_EDGE(next_edge, e1, ABOVE, nx, iy); - VERTEX(e1, ABOVE, LEFT, ix, iy); - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); e0->outp[ABOVE]= e1->outp[ABOVE]; e1->outp[ABOVE]= NULL; } @@ -2218,29 +2218,29 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, case EMX: if (p && q) { - VERTEX(e0, ABOVE, LEFT, ix, iy); + VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); e0->outp[ABOVE]= NULL; e1->outp[ABOVE]= NULL; } break; case IMN: P_EDGE(prev_edge, e0, ABOVE, px, iy); - VERTEX(prev_edge, ABOVE, LEFT, px, iy); + VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); N_EDGE(next_edge, e1, ABOVE, nx, iy); - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); new_tristrip(&tlist, prev_edge, px, iy); e1->outp[ABOVE]= prev_edge->outp[ABOVE]; - VERTEX(e1, ABOVE, RIGHT, ix, iy); + VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); new_tristrip(&tlist, e0, ix, iy); next_edge->outp[ABOVE]= e0->outp[ABOVE]; - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); break; case ILI: if (p) { - VERTEX(e0, ABOVE, LEFT, ix, iy); + VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); N_EDGE(next_edge, e1, ABOVE, nx, iy); - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); e1->outp[ABOVE]= e0->outp[ABOVE]; e0->outp[ABOVE]= NULL; } @@ -2248,9 +2248,9 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, case IRI: if (q) { - VERTEX(e1, ABOVE, RIGHT, ix, iy); + VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); P_EDGE(prev_edge, e0, ABOVE, px, iy); - VERTEX(prev_edge, ABOVE, LEFT, px, iy); + VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); e0->outp[ABOVE]= e1->outp[ABOVE]; e1->outp[ABOVE]= NULL; } @@ -2258,40 +2258,40 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, case IMX: if (p && q) { - VERTEX(e0, ABOVE, RIGHT, ix, iy); - VERTEX(e1, ABOVE, LEFT, ix, iy); + VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); + VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); e0->outp[ABOVE]= NULL; e1->outp[ABOVE]= NULL; P_EDGE(prev_edge, e0, ABOVE, px, iy); - VERTEX(prev_edge, ABOVE, LEFT, px, iy); + VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); new_tristrip(&tlist, prev_edge, px, iy); N_EDGE(next_edge, e1, ABOVE, nx, iy); - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); next_edge->outp[ABOVE]= prev_edge->outp[ABOVE]; - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); } break; case IMM: if (p && q) { - VERTEX(e0, ABOVE, RIGHT, ix, iy); - VERTEX(e1, ABOVE, LEFT, ix, iy); + VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); + VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); P_EDGE(prev_edge, e0, ABOVE, px, iy); - VERTEX(prev_edge, ABOVE, LEFT, px, iy); + VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); new_tristrip(&tlist, prev_edge, px, iy); N_EDGE(next_edge, e1, ABOVE, nx, iy); - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); e1->outp[ABOVE]= prev_edge->outp[ABOVE]; - VERTEX(e1, ABOVE, RIGHT, ix, iy); + VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); new_tristrip(&tlist, e0, ix, iy); next_edge->outp[ABOVE]= e0->outp[ABOVE]; - VERTEX(next_edge, ABOVE, RIGHT, nx, iy); + VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); } break; case EMM: if (p && q) { - VERTEX(e0, ABOVE, LEFT, ix, iy); + VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); new_tristrip(&tlist, e1, ix, iy); e0->outp[ABOVE]= e1->outp[ABOVE]; } @@ -2408,13 +2408,13 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, v= 0; if (INVERT_TRISTRIPS) { - lt= tn->v[RIGHT]; - rt= tn->v[LEFT]; + lt= tn->v[GPC_RIGHT]; + rt= tn->v[GPC_LEFT]; } else { - lt= tn->v[LEFT]; - rt= tn->v[RIGHT]; + lt= tn->v[GPC_LEFT]; + rt= tn->v[GPC_RIGHT]; } while (lt || rt) { @@ -2442,12 +2442,12 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, else { /* Invalid tristrip: just free the heap */ - for (lt= tn->v[LEFT]; lt; lt= ltn) + for (lt= tn->v[GPC_LEFT]; lt; lt= ltn) { ltn= lt->next; FREE(lt); } - for (rt= tn->v[RIGHT]; rt; rt=rtn) + for (rt= tn->v[GPC_RIGHT]; rt; rt=rtn) { rtn= rt->next; FREE(rt); diff --git a/main.cpp b/main.cpp index 57e1bae..8657c03 100755 --- a/main.cpp +++ b/main.cpp @@ -36,7 +36,8 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "*RayTrace3*"; + ::testing::GTEST_FLAG(filter) = "Geo_*"; + //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; //::testing::GTEST_FLAG(filter) = "*BVH*"; diff --git a/math/DrawList.h b/math/DrawList.h index 639205c..dd034ad 100644 --- a/math/DrawList.h +++ b/math/DrawList.h @@ -58,7 +58,7 @@ public: } /** ctor with custom seed */ - DrawList(const uint32_t seed) : cumProbability(0), gen(defRndGen(seed)) { + DrawList(const uint32_t seed) : cumProbability(0), defRndGen(seed), gen(defRndGen) { ; } diff --git a/tests/geo/TestAngle.cpp b/tests/geo/TestAngle.cpp index 0299627..6b0c17d 100755 --- a/tests/geo/TestAngle.cpp +++ b/tests/geo/TestAngle.cpp @@ -4,7 +4,7 @@ #include "../../geo/Angle.h" -TEST(Angle, dir) { +TEST(Geo_Angle, dir) { // angle -> pointer -> angle ASSERT_NEAR(0, Angle::getRAD_2PI(Angle::getPointer(0)), 0.0001); @@ -14,7 +14,7 @@ TEST(Angle, dir) { } -TEST(Angle, safe) { +TEST(Geo_Angle, safe) { ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(0))))); ASSERT_EQ(0, (int)std::round(Angle::radToDeg(Angle::makeSafe_2PI(Angle::degToRad(360))))); @@ -35,7 +35,7 @@ TEST(Angle, safe) { } -TEST(Angle, calc) { +TEST(Geo_Angle, calc) { ASSERT_EQ(0, (int)Angle::getDEG_360(0,0, +1,0)); // to the right ASSERT_EQ(90, (int)Angle::getDEG_360(0,0, 0,+1)); // upwards @@ -49,7 +49,7 @@ TEST(Angle, calc) { } -TEST(Angle, signedDiff) { +TEST(Geo_Angle, signedDiff) { const float d = 0.00001f; ASSERT_NEAR(+M_PI/2, Angle::getSignedDiffRAD_2PI(0, M_PI/2), d); // CCW @@ -62,7 +62,7 @@ TEST(Angle, signedDiff) { } -TEST(Angle, diff) { +TEST(Geo_Angle, diff) { const float r = Angle::getRAD_2PI(0,0, +1,0); // to the right const float u = Angle::getRAD_2PI(0,0, 0,+1); // upwards diff --git a/tests/geo/TestBBox.cpp b/tests/geo/TestBBox2.cpp similarity index 91% rename from tests/geo/TestBBox.cpp rename to tests/geo/TestBBox2.cpp index f8d4d46..4614897 100644 --- a/tests/geo/TestBBox.cpp +++ b/tests/geo/TestBBox2.cpp @@ -4,7 +4,7 @@ #include "../../geo/BBox2.h" -TEST(BBox, noIntersect) { +TEST(Geo_BBox2, noIntersect) { BBox2 bb(Point2(1,1), Point2(3,3)); @@ -21,7 +21,8 @@ TEST(BBox, noIntersect) { } -TEST(BBox, noDirectIntersect) { +/* +TEST(Geo_BBox2, noDirectIntersect) { BBox2 bb(Point2(1,1), Point2(3,3)); @@ -37,9 +38,10 @@ TEST(BBox, noDirectIntersect) { ASSERT_FALSE(bb.intersects(l4)); } +*/ -TEST(BBox, positiveIntersect) { +TEST(Geo_BBox2, positiveIntersect) { BBox2 bb(Point2(1,1), Point2(3,3)); diff --git a/tests/geo/TestBBox3.cpp b/tests/geo/TestBBox3.cpp new file mode 100644 index 0000000..cc769ee --- /dev/null +++ b/tests/geo/TestBBox3.cpp @@ -0,0 +1,18 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/BBox3.h" + +TEST(Geo_BBox3, noIntersect) { + + BBox3 bb1(Point3(0,0,0), Point3(1,1,1)); + BBox3 bb2(Point3(1,1,1), Point3(2,3,4)); + + BBox3 joined = BBox3::join(bb1, bb2); + ASSERT_EQ(Point3(2,3,4), joined.getMax()); + ASSERT_EQ(Point3(0,0,0), joined.getMin()); + +} + +#endif diff --git a/tests/geo/TestBVH2.cpp b/tests/geo/TestBVH2.cpp new file mode 100644 index 0000000..d3245d6 --- /dev/null +++ b/tests/geo/TestBVH2.cpp @@ -0,0 +1,176 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/volume/BVH.h" +#include "../../geo/volume/BVHDebug.h" + +#include "../../geo/BBox2.h" +#include "../../geo/Line2.h" + +#include "../../floorplan/v2/Floorplan.h" +#include "../../floorplan/v2/FloorplanReader.h" +#include "../../wifi/estimate/ray3/ModelFactory.h" + +struct WrapperBBox2 { + + static std::vector getVertices(const BBox2& bbox) { + return {bbox.getMin(), bbox.getMax()}; + } + + static std::vector getDebugLines(const BBox2& bbox) { + + Point2 p1(bbox.getMin().x, bbox.getMin().y); + Point2 p2(bbox.getMax().x, bbox.getMin().y); + Point2 p3(bbox.getMax().x, bbox.getMax().y); + Point2 p4(bbox.getMin().x, bbox.getMax().y); + + std::vector res; + res.push_back(p1); res.push_back(p2); + res.push_back(p2); res.push_back(p3); + res.push_back(p3); res.push_back(p4); + res.push_back(p4); res.push_back(p1); + return res; + + } + +}; + +struct WrapperLine2 { + + static std::vector getVertices(const Line2& l) { + return {l.p1, l.p2}; + } + + static std::vector getDebugLines(const Line2& l) { + return {l.p1, l.p2}; + } + +}; + + +/* +TEST(Geo_BVH2, tree) { + + BVH3Debug tree; + + BBox3 bb1(Point3(0,0,0), Point3(1,1,1)); + tree.add(bb1); + + BBox3 bb2(Point3(-1,-1,-1), Point3(0,0,0)); + tree.add(bb2); + + tree.optimize(); + tree.show(); + + int i = 0; (void) i; + +} + +TEST(Geo_BVH2, tree2) { + + BVH3Debug tree; + + BBox3 bb1(Point3(0,0,0), Point3(1,1,1)); + tree.add(bb1); + + BBox3 bb2(Point3(-1,0,0), Point3(0,1,1)); + tree.add(bb2); + + tree.optimize(); + tree.show(); + + int i = 0; (void) i; + +} + +TEST(Geo_BVH2, tree3) { + + BVH3Debug tree; + + BBox3 bb1 = BBox3::around(Point3(+0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25)); + tree.add(bb1); + + BBox3 bb2 = BBox3::around(Point3(-0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25)); + tree.add(bb2); + + BBox3 bb3 = BBox3::around(Point3(-0.0, +0.5, 0.0), Point3(0.36, 0.36, 0.36)); + tree.add(bb3); + + BBox3 bb4 = BBox3::around(Point3(-0.0, +0.0, 0.0), Point3(0.5, 0.5, 0.5)); + tree.add(bb4); + + tree.optimize(1); + tree.show(); + + tree.optimize(1); + tree.show(); + + tree.optimize(1); + tree.show(); + + int i = 0; (void) i; + +} + +TEST(Geo_BVH2, treeMap) { + + std::string file = "/apps/SHL39.xml"; + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + + ModelFactory fac(map); + fac.setExportCeilings(false); + fac.setFloors({map->floors[3]}); + std::vector obs = fac.triangulize(); + + BVH3Debug tree; + + for (const Obstacle3D& o : obs) { + tree.add(o); + } + + //tree.show(150); + + //int rounds = tree.optimize(); + + for (int i = 0; i < 200; ++i) { + tree.optimize(1); + //if (i%3==0) { + tree.show(250, false); + //} + } + + int i = 0; (void) i; + +} +*/ +TEST(Geo_BVH2, treeRandom) { + + //BVH2Debug tree; + BVH2Debug tree; + + std::minstd_rand gen; + std::uniform_real_distribution dPos(-4.0, +4.0); + std::uniform_real_distribution dDir(+0.1, +0.5); + + for (int i = 0; i < 50; ++i) { + const Point2 pos(dPos(gen), dPos(gen)); + const Point2 dir(dDir(gen), dDir(gen)); + const Line2 line(pos, pos+dir); + tree.add(line); + } + + tree.show(); + + if (1 == 0) { + for (int i = 0; i < 100; ++i) { + tree.optimize(1); + tree.show(); + usleep(1000*100); + } + } + int i = 0; (void) i; + +} + +#endif diff --git a/tests/geo/TestBVH.cpp b/tests/geo/TestBVH3.cpp similarity index 93% rename from tests/geo/TestBVH.cpp rename to tests/geo/TestBVH3.cpp index 83acca2..9d7196e 100644 --- a/tests/geo/TestBVH.cpp +++ b/tests/geo/TestBVH3.cpp @@ -77,7 +77,7 @@ struct WrapperObs3D { TEST(BVH, tree) { - BVHDebug tree; + BVH3Debug tree; BBox3 bb1(Point3(0,0,0), Point3(1,1,1)); tree.add(bb1); @@ -94,7 +94,7 @@ TEST(BVH, tree) { TEST(BVH, tree2) { - BVHDebug tree; + BVH3Debug tree; BBox3 bb1(Point3(0,0,0), Point3(1,1,1)); tree.add(bb1); @@ -111,7 +111,7 @@ TEST(BVH, tree2) { TEST(BVH, tree3) { - BVHDebug tree; + BVH3Debug tree; BBox3 bb1 = BBox3::around(Point3(+0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25)); tree.add(bb1); @@ -148,7 +148,7 @@ TEST(BVH, treeMap) { fac.setFloors({map->floors[3]}); std::vector obs = fac.triangulize(); - BVHDebug tree; + BVH3Debug tree; for (const Obstacle3D& o : obs) { tree.add(o); @@ -171,7 +171,7 @@ TEST(BVH, treeMap) { TEST(BVH, treeRandom) { - BVHDebug tree; + BVH3Debug tree; std::minstd_rand gen; std::uniform_real_distribution dPos(-4.0, +4.0); diff --git a/tests/geo/TestCircle2.cpp b/tests/geo/TestCircle2.cpp new file mode 100644 index 0000000..a54f4c5 --- /dev/null +++ b/tests/geo/TestCircle2.cpp @@ -0,0 +1,85 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../geo/Circle2.h" + +TEST(Geo_Circle2, intersect) { + + Circle2 c1(Point2(3,2), 1.0); + + // left of circle + Ray2 r1(Point2(1,0), Point2(0,1)); + ASSERT_FALSE(c1.intersects(r1)); + + // right of circle + Ray2 r2(Point2(5,0), Point2(0,1)); + ASSERT_FALSE(c1.intersects(r2)); + + + // through circle (from bottom) + Ray2 r3(Point2(3,0), Point2(0,1)); + ASSERT_TRUE(c1.intersects(r3)); + // through circle (from bottom) + Ray2 r4(Point2(3.5,0), Point2(0,1)); + ASSERT_TRUE(c1.intersects(r4)); + + // within circle + Ray2 r5(Point2(3,2), Point2(0,1)); + ASSERT_TRUE(c1.intersects(r5)); + + + // through circle (from left) + Ray2 r6(Point2(0,2), Point2(1,0)); + ASSERT_TRUE(c1.intersects(r6)); + // through circle (from right) + Ray2 r7(Point2(10,2), Point2(-1,0)); + ASSERT_TRUE(c1.intersects(r7)); + // through circle (from top) + Ray2 r8(Point2(3,10), Point2(0,-1)); + ASSERT_TRUE(c1.intersects(r8)); + + +} + +TEST(Geo_Circle2, contains) { + + Circle2 c(Point2(0,0), 1.01); + + ASSERT_TRUE(c.contains(Point2(+1,0))); + ASSERT_TRUE(c.contains(Point2(-1,0))); + + ASSERT_TRUE(c.contains(Point2(0,+1))); + ASSERT_TRUE(c.contains(Point2(0,-1))); + + ASSERT_FALSE(c.contains(Point2(+1,+1))); + ASSERT_FALSE(c.contains(Point2(-1,-1))); + +} + +TEST(Geo_Circle2, join) { + + // no overlap + Circle2 a1(Point2(-1,0), 0.5); + Circle2 a2(Point2(+1,0), 0.5); + Circle2 a3 = Circle2::join(a1,a2); + ASSERT_EQ(Point2(0,0), a3.center); + ASSERT_NEAR(1.5, a3.radius, 0.01); + + // overlap + Circle2 b1(Point2(0,+1), 1.5); + Circle2 b2(Point2(0,-1), 1.5); + Circle2 b3 = Circle2::join(b1,b2); + ASSERT_EQ(Point2(0,0), b3.center); + ASSERT_NEAR(2.5, b3.radius, 0.01); + + // within + Circle2 c1(Point2(0,+1), 3.0); + Circle2 c2(Point2(0,-1), 1.0); + Circle2 c3 = Circle2::join(c1,c2); + ASSERT_EQ(c1.center, c3.center); + ASSERT_NEAR(c1.radius, c3.radius, 0.01); + +} + +#endif diff --git a/tests/geo/TestHeading.cpp b/tests/geo/TestHeading.cpp index 3343ca9..9de850d 100644 --- a/tests/geo/TestHeading.cpp +++ b/tests/geo/TestHeading.cpp @@ -3,7 +3,7 @@ #include "../Tests.h" #include "../../geo/Heading.h" -TEST(Heading, diff) { +TEST(Geo_Heading, diff) { // 180 degree turn { @@ -26,7 +26,7 @@ TEST(Heading, diff) { } -TEST(Heading, mod) { +TEST(Geo_Heading, mod) { const float d = 0.0001; @@ -36,7 +36,7 @@ TEST(Heading, mod) { } -TEST(Heading, ctor) { +TEST(Geo_Heading, ctor) { // OK Heading(0); @@ -54,7 +54,7 @@ TEST(Heading, ctor) { } -TEST(Heading, eq) { +TEST(Geo_Heading, eq) { ASSERT_EQ(Heading(0), Heading(0)); ASSERT_EQ(Heading(1), Heading(1)); diff --git a/tests/geo/TestLength.cpp b/tests/geo/TestLength.cpp index f4ca382..2d5fd39 100644 --- a/tests/geo/TestLength.cpp +++ b/tests/geo/TestLength.cpp @@ -3,7 +3,7 @@ #include "../Tests.h" #include "../../geo/Length.h" -TEST(Length, float) { +TEST(Geo_Length, float) { static constexpr float delta = 0.00001; @@ -24,7 +24,7 @@ TEST(Length, float) { } -TEST(Length, int) { +TEST(Geo_Length, int) { LengthI l1 = LengthI::m(1.0); ASSERT_EQ(1, l1.m()); diff --git a/tests/geo/TestPoint2.cpp b/tests/geo/TestPoint2.cpp new file mode 100644 index 0000000..e1adbc5 --- /dev/null +++ b/tests/geo/TestPoint2.cpp @@ -0,0 +1,23 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../geo/Point2.h" + +TEST(Geo_Point2, math) { + + Point2 p1(1,2); + p1 += Point2(2,3); + ASSERT_EQ(p1, Point2(3,5)); + + Point2 p2 = Point2(-2,-1) + p1; + ASSERT_EQ(p2, Point2(1, 4)); + + p2 -= Point2(1, 2); + ASSERT_EQ(p2, Point2(0,2)); + + Point2 p3 = Point2(1,2)*2; + ASSERT_EQ(p3, Point2(2,4)); + +} + +#endif diff --git a/tests/geo/TestPoint.cpp b/tests/geo/TestPoint3.cpp similarity index 93% rename from tests/geo/TestPoint.cpp rename to tests/geo/TestPoint3.cpp index 07b44e7..33bd0ac 100644 --- a/tests/geo/TestPoint.cpp +++ b/tests/geo/TestPoint3.cpp @@ -3,7 +3,7 @@ #include "../Tests.h" #include "../../geo/Point3.h" -TEST(Point3, math) { +TEST(Geo_Point3, math) { Point3 p1(1,2,3); p1 += Point3(2,3,4); diff --git a/tests/geo/TestSphere3.cpp b/tests/geo/TestSphere3.cpp index 937ff65..d74f845 100644 --- a/tests/geo/TestSphere3.cpp +++ b/tests/geo/TestSphere3.cpp @@ -4,7 +4,7 @@ #include "../../geo/Sphere3.h" -TEST(Sphere3, contains) { +TEST(Geo_Sphere3, contains) { Sphere3 a(Point3(0,0,0), 10); @@ -30,31 +30,31 @@ TEST(Sphere3, contains) { } -TEST(Sphere3, join) { +TEST(Geo_Sphere3, join) { // no overlap Sphere3 a1(Point3(-1,0,0), 1); Sphere3 a2(Point3(+1,0,0), 1); Sphere3 a3 = Sphere3::join(a1, a2); ASSERT_EQ(Point3(0,0,0), a3.center); - ASSERT_EQ(2, a3.radius); + ASSERT_NEAR(2, a3.radius, 0.001); // overlap Sphere3 b1(Point3(-1,0,0), 1.5); Sphere3 b2(Point3(+1,0,0), 1.5); Sphere3 b3 = Sphere3::join(b1, b2); ASSERT_EQ(Point3(0,0,0), b3.center); - ASSERT_EQ(2.5, b3.radius); + ASSERT_NEAR(2.5, b3.radius, 0.001); // fully within Sphere3 c1(Point3(-1,0,0), 3.6); Sphere3 c2(Point3(+1,0,0), 1.5); Sphere3 c3 = Sphere3::join(c1, c2); ASSERT_EQ(c1.center, c3.center); - ASSERT_EQ(c1.radius, c3.radius); + ASSERT_NEAR(c1.radius, c3.radius, 0.001); Sphere3 c4 = Sphere3::join(c2, c1); ASSERT_EQ(c1.center, c4.center); - ASSERT_EQ(c1.radius, c4.radius); + ASSERT_NEAR(c1.radius, c4.radius, 0.001); } diff --git a/tests/geo/TestTriangle3.cpp b/tests/geo/TestTriangle3.cpp index fe3b892..6ef2649 100644 --- a/tests/geo/TestTriangle3.cpp +++ b/tests/geo/TestTriangle3.cpp @@ -5,7 +5,7 @@ // https://stackoverflow.com/questions/17458562/efficient-aabb-triangle-intersection-in-c-sharp // http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/tribox3.txt -TEST(Triangle3, intersect) { +TEST(Geo_Triangle3, intersect) { Point3 dst; diff --git a/tests/ray/TestRayTrace2.cpp b/tests/ray/TestRayTrace2.cpp new file mode 100644 index 0000000..6a92ceb --- /dev/null +++ b/tests/ray/TestRayTrace2.cpp @@ -0,0 +1,37 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" +#include "../../wifi/estimate/ray2d/WiFiRayTrace2D.h" +#include "../../floorplan/v2/FloorplanReader.h" +#include + +TEST(RayTrace2, test) { + + //std::string file = "/mnt/data/workspaces/raytest2.xml"; + //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + //Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[0]; + + //std::string file = "/apps/SHL39.xml"; + std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; + Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); + Floorplan::Floor* floor = map->floors[0]; + Floorplan::AccessPoint* ap = floor->accesspoints[4]; + +// ModelFactory fac(map); +// std::ofstream outOBJ("/tmp/vm/map.obj"); +// outOBJ << fac.toOBJ(); +// outOBJ.close(); + + const int gs_cm = 50; + + WiFiRaytrace2D rt(floor, gs_cm, ap->pos.xy()); + + std::chrono::time_point start = std::chrono::high_resolution_clock::now(); + const DataMapSignal& dms = rt.estimate(); + std::chrono::time_point end = std::chrono::high_resolution_clock::now(); + auto result = std::chrono::duration_cast(end-start).count(); + std::cout << "it took: " << result << " msec" << std::endl; + +} + +#endif diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp index 55cee92..c88a7f3 100644 --- a/tests/ray/TestRayTrace3.cpp +++ b/tests/ray/TestRayTrace3.cpp @@ -12,7 +12,8 @@ TEST(RayTrace3, test) { //Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); //Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[0]; - std::string file = "/apps/SHL39.xml"; + //std::string file = "/apps/SHL39.xml"; + std::string file = "/mnt/data/workspaces/IndoorMap/maps/SHL39.xml"; Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[4]; diff --git a/wifi/estimate/ray2d/DataMap2.h b/wifi/estimate/ray2d/DataMap2.h index 2a38a1e..79b11d8 100644 --- a/wifi/estimate/ray2d/DataMap2.h +++ b/wifi/estimate/ray2d/DataMap2.h @@ -3,6 +3,7 @@ #include "../../../geo/BBox2.h" #include +#include template class DataMap { diff --git a/wifi/estimate/ray2d/Ray2.h b/wifi/estimate/ray2d/Ray2.h deleted file mode 100644 index 258f066..0000000 --- a/wifi/estimate/ray2d/Ray2.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef RAY2_H -#define RAY2_H - -#include "../../../geo/Point2.h" - -struct Ray2 { - - /** starting position */ - Point2 start; - - /** ray direction */ - Point2 dir; - - /** empty ctor */ - Ray2() {;} - - /** ctor */ - Ray2(const Point2 start, const Point2 dir) : start(start), dir(dir.normalized()) { - ; - } - -}; - -#endif // RAY2_H diff --git a/wifi/estimate/ray2d/WiFiRayTrace2D.h b/wifi/estimate/ray2d/WiFiRayTrace2D.h index f85eace..928ec4b 100644 --- a/wifi/estimate/ray2d/WiFiRayTrace2D.h +++ b/wifi/estimate/ray2d/WiFiRayTrace2D.h @@ -4,12 +4,14 @@ #include "../../../geo/Point2.h" #include "../../../geo/Line2.h" #include "../../../geo/BBox2.h" +#include "../../../geo/Ray2.h" #include "../../../floorplan/v2/Floorplan.h" #include "../../../floorplan/v2/FloorplanHelper.h" +#include "../../../geo/volume/BVHDebug.h" + #include "DataMap2.h" -#include "Ray2.h" #include "MaterialOptions.h" #include @@ -24,6 +26,38 @@ + + + + +struct Obstacle2D { + Floorplan::Material mat; + Line2 line; + Obstacle2D(Floorplan::Material mat, Line2 line) : mat(mat), line(line) {;} +}; + + +struct Obstacle2DWrapper { + static std::vector getVertices(const Obstacle2D& obs) { + return {obs.line.p1, obs.line.p2}; + } + + static std::vector getDebugLines(const Obstacle2D& obs) { + return {obs.line.p1, obs.line.p2}; + } +}; + +struct Hit { + const Obstacle2D* obstacle; + float dist; + Point2 pos; + Point2 normal; + Floorplan::Material material; + bool stopHere = false; + Hit() {;} + Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;} +}; + struct StateRay2 : public Ray2 { /** already travelled distance from the AP (by all previous rays */ @@ -32,7 +66,7 @@ struct StateRay2 : public Ray2 { /** attenuation taken since the start */ float totalAttenuation; - void* lastObstacle; + const Obstacle2D* lastObstacle; int depth = 0; /** empty ctor */ @@ -51,17 +85,6 @@ struct StateRay2 : public Ray2 { }; -struct Hit { - void* obstacle; - float dist; - Point2 pos; - Point2 normal; - Floorplan::Material material; - bool stopHere = false; - Hit() {;} - Hit(const float dist, const Point2 pos, const Point2 normal) : dist(dist), pos(pos), normal(normal) {;} -}; - class WiFiRaytrace2D { @@ -72,11 +95,13 @@ private: BBox2 bbox; Point2 apPos; + BVH2Debug tree; + DataMapSignal dm; struct Limit { static constexpr int RAYS = 2000; - static constexpr int HITS = 11; + static constexpr int HITS = 25; static constexpr float RSSI = -110; }; @@ -94,6 +119,34 @@ public: // allocate dm.resize(bbox, gs); + // build tree + for (Floorplan::FloorObstacle* fo : floor->obstacles) { + + const Floorplan::FloorObstacleLine* line = dynamic_cast(fo); + const Floorplan::FloorObstacleDoor* door = dynamic_cast(fo); + + if (line) { + Obstacle2D obs(line->material, Line2(line->from, line->to)); + tree.add(obs, true); + } else if (door) { + Obstacle2D obs(door->material, Line2(door->from, door->to)); + tree.add(obs, true); + } + + } + +// for (int i = 0; i < 200; ++i) { +// tree.optimize(1); +// tree.show(1500, false); +// usleep(1000*100); +// } + +// tree.show(); + tree.optimize(250); +// int depth = tree.getDepth(); + tree.show(1500,false); + + } const DataMapSignal& estimate() { @@ -139,17 +192,13 @@ private: // continue? if (hit.stopHere) {return;} - //const float curLength = ray.totalLength + hit.dist; - //if (curLength > 55) {return;} if (ray.getRSSI(hit.dist) < Limit::RSSI) {return;} if (ray.depth > Limit::HITS) {return;} - // apply effects - //reflected(ray, hit); + reflected(ray, hit); shadowed(ray, hit); - } static inline float getAttenuation(const Hit& h) { @@ -208,43 +257,46 @@ private: } + static inline void hitTest(const Line2& longRay, const Obstacle2D& obs, Hit& nearest) { - Hit getNearestHit(const StateRay2& ray) { + const float minDist = 0.01; // prevent errors hitting the same obstacle twice + + // do not hit the last obstacle again + //if (ray.lastObstacle == fo) {continue;} + + // get the line + Point2 hit; + if (obs.line.getSegmentIntersection(longRay, hit)) { + const float dist = hit.getDistance(longRay.p1); + if (dist > minDist && dist < nearest.dist) { + nearest.obstacle = &obs; + nearest.dist = dist; + nearest.pos = hit; + nearest.normal = (obs.line.p2 - obs.line.p1).perpendicular().normalized(); + nearest.material = obs.mat; + } + } + + } + + Hit getNearestHit(const StateRay2& ray) const { Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!"); - const Line2 longRay(ray.start, ray.start + ray.dir*100); + const Line2 longRay(ray.start, ray.start + ray.dir*1000); - const float minDist = 0;//0.01; // prevent errors hitting the same obstacle twice + //const float minDist = 0;//0.01; // prevent errors hitting the same obstacle twice const float MAX = 999999; Hit nearest; nearest.dist = MAX; - // check intersection with walls - for (Floorplan::FloorObstacle* fo : floor->obstacles) { + //int hits = 0; - // do not hit the last obstacle again - if (ray.lastObstacle == fo) {continue;} + const auto onHit = [longRay, &nearest] (const Obstacle2D& obs) { + //++hits; + hitTest(longRay, obs, nearest); + }; - // get the line - const Floorplan::FloorObstacleLine* line = dynamic_cast(fo); - const Floorplan::FloorObstacleDoor* door = dynamic_cast(fo); - if (!line && !door) {continue;} - Line2 obstacle; - if (line) {obstacle = Line2(line->from, line->to);} - if (door) {obstacle = Line2(door->from, door->to);} - - Point2 hit; - if (obstacle.getSegmentIntersection(longRay, hit)) { - const float dist = hit.getDistance(ray.start); - if (dist > minDist && dist < nearest.dist) { - nearest.obstacle = fo; - nearest.dist = dist; - nearest.pos = hit; - nearest.normal = (obstacle.p2 - obstacle.p1).perpendicular().normalized(); - nearest.material = fo->material; - } - } - } + tree.getHits(ray, onHit); // no hit with floorplan: limit to bounding-box! if (nearest.dist == MAX) { diff --git a/wifi/estimate/ray3/WifiRayTrace3D.h b/wifi/estimate/ray3/WifiRayTrace3D.h index c94d7bd..fabb3e8 100644 --- a/wifi/estimate/ray3/WifiRayTrace3D.h +++ b/wifi/estimate/ray3/WifiRayTrace3D.h @@ -70,6 +70,7 @@ struct StateRay3 : public Ray3 { StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const { + (void) obs; StateRay3 next = getNext(hitPos); next.isWithin = nullptr; return next; @@ -173,7 +174,7 @@ private: DataMap3Signal dm; - BVHDebug tree; + BVH3Debug tree; struct Limit { static constexpr int RAYS = 15000;