added support for .obj objects within the floorplan
include objects within navmesh calculation include objects within 3d mesh generation minor changes/fixes
This commit is contained in:
@@ -19,7 +19,6 @@ ENDIF()
|
||||
|
||||
INCLUDE_DIRECTORIES(
|
||||
../
|
||||
/mnt/firma/kunden/HandyGames/
|
||||
)
|
||||
|
||||
|
||||
@@ -85,7 +84,7 @@ ADD_DEFINITIONS(
|
||||
-DWITH_TESTS
|
||||
-DWITH_ASSERTIONS
|
||||
-DWITH_DEBUG_LOG
|
||||
-D_GLIBCXX_DEBUG
|
||||
-D_GLIBCXX_DEBUG
|
||||
|
||||
|
||||
)
|
||||
|
||||
@@ -275,15 +275,15 @@ namespace Floorplan {
|
||||
bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);}
|
||||
};
|
||||
|
||||
/** a GroundTruthPoint located somewhere on a floor */
|
||||
struct GroundTruthPoint {
|
||||
int id; //TODO: this value can be changed and isn't set incremental within the indoormap
|
||||
/** a GroundTruthPoint located somewhere on a floor */
|
||||
struct GroundTruthPoint {
|
||||
int id; //TODO: this value can be changed and isn't set incremental within the indoormap
|
||||
Point3 pos; // TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
|
||||
GroundTruthPoint() : id(), pos() {;}
|
||||
GroundTruthPoint() : id(), pos() {;}
|
||||
GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;}
|
||||
const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
|
||||
bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
|
||||
};
|
||||
bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
|
||||
};
|
||||
|
||||
/** an AccessPoint located somewhere on a floor */
|
||||
struct AccessPoint : public HasMeta {
|
||||
@@ -317,7 +317,7 @@ namespace Floorplan {
|
||||
Beacon() : name(), mac(), pos() {;}
|
||||
Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
|
||||
bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
|
||||
Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground
|
||||
Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);} // relative to the floor's ground
|
||||
};
|
||||
|
||||
|
||||
@@ -371,6 +371,13 @@ namespace Floorplan {
|
||||
float getSize() const {return (to-from).length();}
|
||||
};
|
||||
|
||||
/** 3D obstacle */
|
||||
struct FloorObstacleObject : public FloorObstacle {
|
||||
std::string file;
|
||||
Point3 pos;
|
||||
Point3 rot;
|
||||
FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot) : file(file), pos(pos), rot(rot) {;}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -391,6 +391,7 @@ namespace Floorplan {
|
||||
if (std::string("line") == n->Name()) {obstacles.push_back(parseFloorObstacleLine(n));}
|
||||
if (std::string("circle") == n->Name()) {obstacles.push_back(parseFloorObstacleCircle(n));}
|
||||
if (std::string("door") == n->Name()) {obstacles.push_back(parseFloorObstacleDoor(n));}
|
||||
if (std::string("object") == n->Name()) {obstacles.push_back(parseFloorObstacleObject(n));}
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
@@ -426,6 +427,15 @@ namespace Floorplan {
|
||||
);
|
||||
}
|
||||
|
||||
/** parse one object */
|
||||
static FloorObstacleObject* parseFloorObstacleObject(const XMLElem* el) {
|
||||
return new FloorObstacleObject(
|
||||
el->Attribute("file"),
|
||||
Point3(el->FloatAttribute("x"), el->FloatAttribute("y"), el->FloatAttribute("z")),
|
||||
Point3(el->FloatAttribute("rx"), el->FloatAttribute("ry"), el->FloatAttribute("rz"))
|
||||
);
|
||||
}
|
||||
|
||||
/** parse a floor's <outline> tag */
|
||||
static FloorOutline parseFloorOutline(const XMLElem* el) {
|
||||
FloorOutline outline;
|
||||
|
||||
@@ -170,16 +170,16 @@ namespace Floorplan {
|
||||
}
|
||||
floor->InsertEndChild(pois);
|
||||
|
||||
XMLElem* gtpoints = doc.NewElement("gtpoints");
|
||||
for (const GroundTruthPoint* gtp : mf->gtpoints) {
|
||||
XMLElem* elem = doc.NewElement("gtpoint");
|
||||
elem->SetAttribute("id", gtp->id);
|
||||
elem->SetAttribute("x", gtp->pos.x);
|
||||
elem->SetAttribute("y", gtp->pos.y);
|
||||
elem->SetAttribute("z", gtp->pos.z);
|
||||
gtpoints->InsertEndChild(elem);
|
||||
}
|
||||
floor->InsertEndChild(gtpoints);
|
||||
XMLElem* gtpoints = doc.NewElement("gtpoints");
|
||||
for (const GroundTruthPoint* gtp : mf->gtpoints) {
|
||||
XMLElem* elem = doc.NewElement("gtpoint");
|
||||
elem->SetAttribute("id", gtp->id);
|
||||
elem->SetAttribute("x", gtp->pos.x);
|
||||
elem->SetAttribute("y", gtp->pos.y);
|
||||
elem->SetAttribute("z", gtp->pos.z);
|
||||
gtpoints->InsertEndChild(elem);
|
||||
}
|
||||
floor->InsertEndChild(gtpoints);
|
||||
|
||||
XMLElem* accesspoints = doc.NewElement("accesspoints");
|
||||
for (const AccessPoint* ap : mf->accesspoints) {
|
||||
@@ -315,6 +315,8 @@ namespace Floorplan {
|
||||
addFloorObstacleCircle(doc, obstacles, (FloorObstacleCircle*)fo);
|
||||
} else if (dynamic_cast<FloorObstacleDoor*>(fo)) {
|
||||
addFloorObstacleDoor(doc, obstacles, (FloorObstacleDoor*)fo);
|
||||
} else if (dynamic_cast<FloorObstacleObject*>(fo)) {
|
||||
addFloorObstacleObject(doc, obstacles, (FloorObstacleObject*)fo);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -359,6 +361,19 @@ namespace Floorplan {
|
||||
obstacles->InsertEndChild(obstacle);
|
||||
}
|
||||
|
||||
/** write an object-obstacle */
|
||||
static void addFloorObstacleObject(XMLDoc& doc, XMLElem* obstacles, FloorObstacleObject* obj) {
|
||||
XMLElem* obstacle = doc.NewElement("object");
|
||||
obstacle->SetAttribute("file", obj->file.c_str());
|
||||
obstacle->SetAttribute("x", obj->pos.x);
|
||||
obstacle->SetAttribute("y", obj->pos.y);
|
||||
obstacle->SetAttribute("z", obj->pos.z);
|
||||
obstacle->SetAttribute("rx", obj->rot.x);
|
||||
obstacle->SetAttribute("ry", obj->rot.y);
|
||||
obstacle->SetAttribute("rz", obj->rot.z);
|
||||
obstacles->InsertEndChild(obstacle);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
73
geo/BBox2.h
73
geo/BBox2.h
@@ -9,8 +9,8 @@ class BBox2 {
|
||||
|
||||
protected:
|
||||
|
||||
static constexpr float MAX = +99999;
|
||||
static constexpr float MIN = -99999;
|
||||
static constexpr float MAX = +99999999;
|
||||
static constexpr float MIN = -99999999;
|
||||
|
||||
/** minimum */
|
||||
Point2 p1;
|
||||
@@ -26,17 +26,28 @@ public:
|
||||
/** ctor */
|
||||
BBox2(const Point2& p1, const Point2& p2) : p1(p1), p2(p2) {;}
|
||||
|
||||
/** ctor */
|
||||
BBox2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;}
|
||||
|
||||
/** adjust the bounding-box by adding this point */
|
||||
void add(const Point2& p) {
|
||||
add(p.x, p.y);
|
||||
}
|
||||
|
||||
if (p.x > p2.x) {p2.x = p.x;}
|
||||
if (p.y > p2.y) {p2.y = p.y;}
|
||||
/** adjust the bounding-box by adding this point */
|
||||
void add(const float x, const float y) {
|
||||
|
||||
if (p.x < p1.x) {p1.x = p.x;}
|
||||
if (p.y < p1.y) {p1.y = p.y;}
|
||||
if (x > p2.x) {p2.x = x;}
|
||||
if (y > p2.y) {p2.y = y;}
|
||||
|
||||
if (x < p1.x) {p1.x = x;}
|
||||
if (y < p1.y) {p1.y = y;}
|
||||
|
||||
}
|
||||
|
||||
/** the area spanned by the bbox */
|
||||
float getArea() const {return getSize().x * getSize().y;}
|
||||
|
||||
/** returns true if the bbox is not yet configured */
|
||||
bool isInvalid() const {
|
||||
return p1.x == MAX || p1.y == MAX || p2.x == MIN || p2.y == MIN;
|
||||
@@ -63,6 +74,44 @@ public:
|
||||
(p2.y == o.p2.y);
|
||||
}
|
||||
|
||||
bool intersects(const BBox2& o) const {
|
||||
// TODO is this correct?
|
||||
if (o.p2.x < p1.x) {return false;}
|
||||
if (o.p1.x > p2.x) {return false;}
|
||||
if (o.p2.y < p1.y) {return false;}
|
||||
if (o.p1.y > p2.y) {return false;}
|
||||
return true;
|
||||
// return (p1.x <= o.p2.x) &&
|
||||
// (p1.y <= o.p2.y) &&
|
||||
// (p2.x >= o.p1.x) &&
|
||||
// (p2.y >= o.p1.y);
|
||||
}
|
||||
|
||||
BBox2 combine(const BBox2& o) {
|
||||
|
||||
// TODO is this correct?
|
||||
const float x1 = std::min(p1.x, o.p1.x);
|
||||
const float x2 = std::max(p2.x, o.p2.x);
|
||||
|
||||
const float y1 = std::min(p1.y, o.p1.y);
|
||||
const float y2 = std::max(p2.y, o.p2.y);
|
||||
|
||||
return BBox2(x1,y1, x2,y2);
|
||||
|
||||
}
|
||||
|
||||
BBox2 intersection(const BBox2& o) {
|
||||
// TODO is this correct?
|
||||
const float x1 = std::max(p1.x, o.p1.x);
|
||||
const float x2 = std::min(p2.x, o.p2.x);
|
||||
|
||||
const float y1 = std::max(p1.y, o.p1.y);
|
||||
const float y2 = std::min(p2.y, o.p2.y);
|
||||
|
||||
return BBox2(x1,y1, x2,y2);
|
||||
|
||||
}
|
||||
|
||||
/** does the BBox intersect with the given line? */
|
||||
bool intersects (const Line2& l) const {
|
||||
const Line2 l1(p1.x, p1.y, p2.x, p1.y); // upper
|
||||
@@ -87,10 +136,14 @@ public:
|
||||
}
|
||||
|
||||
bool contains(const Point2& p) const {
|
||||
if (p.x < p1.x) {return false;}
|
||||
if (p.x > p2.x) {return false;}
|
||||
if (p.y < p1.y) {return false;}
|
||||
if (p.y > p2.y) {return false;}
|
||||
return contains(p.x, p.y);
|
||||
}
|
||||
|
||||
bool contains(const float x, const float y) const {
|
||||
if (x < p1.x) {return false;}
|
||||
if (x > p2.x) {return false;}
|
||||
if (y < p1.y) {return false;}
|
||||
if (y > p2.y) {return false;}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
64
geo/ConvexHull2.h
Normal file
64
geo/ConvexHull2.h
Normal file
@@ -0,0 +1,64 @@
|
||||
#ifndef GEO_CONVEXHULL2_H
|
||||
#define GEO_CONVEXHULL2_H
|
||||
|
||||
#include "Point2.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
/**
|
||||
* get a convex-hull around a set of 2D points
|
||||
* https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
|
||||
*/
|
||||
class ConvexHull2 {
|
||||
|
||||
public:
|
||||
|
||||
//using namespace std;
|
||||
|
||||
typedef double coord_t; // coordinate type
|
||||
typedef double coord2_t; // must be big enough to hold 2*max(|coordinate|)^2
|
||||
|
||||
// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
|
||||
// Returns a positive value, if OAB makes a counter-clockwise turn,
|
||||
// negative for clockwise turn, and zero if the points are collinear.
|
||||
static inline coord2_t cross(const Point2 O, const Point2 A, const Point2 B) {
|
||||
return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
|
||||
}
|
||||
|
||||
// Returns a list of points on the convex hull in counter-clockwise order.
|
||||
// Note: the last point in the returned list is the same as the first one.
|
||||
static inline std::vector<Point2> get(std::vector<Point2> P) {
|
||||
|
||||
auto comp = [] (const Point2 p1, const Point2 p2) {
|
||||
return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
|
||||
};
|
||||
|
||||
int n = P.size(), k = 0;
|
||||
if (n == 1) return P;
|
||||
std::vector<Point2> H(2*n);
|
||||
|
||||
// Sort points lexicographically
|
||||
std::sort(P.begin(), P.end(), comp);
|
||||
|
||||
// Build lower hull
|
||||
for (int i = 0; i < n; ++i) {
|
||||
while (k >= 2 && cross(H[k-2], H[k-1], P[i]) <= 0) k--;
|
||||
H[k++] = P[i];
|
||||
}
|
||||
|
||||
// Build upper hull
|
||||
for (int i = n-2, t = k+1; i >= 0; i--) {
|
||||
while (k >= t && cross(H[k-2], H[k-1], P[i]) <= 0) k--;
|
||||
H[k++] = P[i];
|
||||
}
|
||||
|
||||
H.resize(k-1);
|
||||
return H;
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // GEO_CONVEXHULL2_H
|
||||
89
geo/Point3.h
89
geo/Point3.h
@@ -1,5 +1,5 @@
|
||||
#ifndef GEO_POINT3_H
|
||||
#define GEO_POINT3_H
|
||||
#ifndef GEO_Point3_H
|
||||
#define GEO_Point3_H
|
||||
|
||||
#include "../Assertions.h"
|
||||
#include <cmath>
|
||||
@@ -8,76 +8,76 @@
|
||||
/**
|
||||
* 3D Point
|
||||
*/
|
||||
struct Point3 {
|
||||
template <typename Scalar> struct _Point3 {
|
||||
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
Scalar x;
|
||||
Scalar y;
|
||||
Scalar z;
|
||||
|
||||
/** ctor */
|
||||
Point3() : x(0), y(0), z(0) {;}
|
||||
_Point3() : x(0), y(0), z(0) {;}
|
||||
|
||||
/** ctor */
|
||||
Point3(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
|
||||
_Point3(const Scalar x, const Scalar y, const Scalar z) : x(x), y(y), z(z) {;}
|
||||
|
||||
|
||||
Point3 operator - () const {return Point3(-x, -y, -z);}
|
||||
_Point3 operator - () const {return _Point3(-x, -y, -z);}
|
||||
|
||||
|
||||
Point3 operator + (const Point3& o) const {return Point3(x+o.x, y+o.y, z+o.z);}
|
||||
_Point3 operator + (const _Point3& o) const {return _Point3(x+o.x, y+o.y, z+o.z);}
|
||||
|
||||
Point3 operator - (const Point3& o) const {return Point3(x-o.x, y-o.y, z-o.z);}
|
||||
_Point3 operator - (const _Point3& o) const {return _Point3(x-o.x, y-o.y, z-o.z);}
|
||||
|
||||
Point3 operator * (const Point3& o) const {return Point3(x*o.x, y*o.y, z*o.z);}
|
||||
_Point3 operator * (const _Point3& o) const {return _Point3(x*o.x, y*o.y, z*o.z);}
|
||||
|
||||
Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
|
||||
_Point3 operator * (const Scalar v) const {return _Point3(v*x, v*y, v*z);}
|
||||
|
||||
Point3 operator / (const float v) const {return Point3(x/v, y/v, z/v);}
|
||||
_Point3 operator / (const Scalar v) const {return _Point3(x/v, y/v, z/v);}
|
||||
|
||||
|
||||
Point3& operator *= (const float v) {x*=v; y*=v; z*=v; return *this;}
|
||||
_Point3& operator *= (const Scalar v) {x*=v; y*=v; z*=v; return *this;}
|
||||
|
||||
Point3& operator /= (const float v) {x/=v; y/=v; z/=v; return *this;}
|
||||
_Point3& operator /= (const Scalar v) {x/=v; y/=v; z/=v; return *this;}
|
||||
|
||||
|
||||
Point3& operator += (const Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}
|
||||
_Point3& operator += (const _Point3& o) {x+=o.x; y+=o.y; z+=o.z; return *this;}
|
||||
|
||||
Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
|
||||
_Point3& operator -= (const _Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
|
||||
|
||||
Point3& operator *= (const Point3& o) {x*=o.x; y*=o.y; z*=o.z; return *this;}
|
||||
_Point3& operator *= (const _Point3& o) {x*=o.x; y*=o.y; z*=o.z; return *this;}
|
||||
|
||||
Point3& operator /= (const Point3& o) {x/=o.x; y/=o.y; z/=o.z; return *this;}
|
||||
_Point3& operator /= (const _Point3& o) {x/=o.x; y/=o.y; z/=o.z; return *this;}
|
||||
|
||||
|
||||
bool operator < (const Point3& o) const {return x<o.x && y<o.y && z<o.z;}
|
||||
bool operator < (const _Point3& o) const {return x<o.x && y<o.y && z<o.z;}
|
||||
|
||||
bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
|
||||
bool operator == (const _Point3& o) const {return x==o.x && y==o.y && z==o.z;}
|
||||
|
||||
bool operator != (const Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;}
|
||||
bool operator != (const _Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;}
|
||||
|
||||
bool eq (const Point3& o, const float delta) const { return eq(x,o.x,delta) && eq(y,o.y,delta) && eq(z,o.z,delta); }
|
||||
bool eq (const _Point3& o, const Scalar delta) const { return eq(x,o.x,delta) && eq(y,o.y,delta) && eq(z,o.z,delta); }
|
||||
|
||||
|
||||
Point2 xy() const {return Point2(x,y);}
|
||||
|
||||
|
||||
Point3 rotX(const float r) const {
|
||||
return Point3(x, y*cos(r) - z*sin(r), y*sin(r) + z*cos(r));
|
||||
_Point3 rotX(const Scalar r) const {
|
||||
return _Point3(x, y*cos(r) - z*sin(r), y*sin(r) + z*cos(r));
|
||||
}
|
||||
Point3 rotY(const float r) const {
|
||||
return Point3(z*sin(r) + x*cos(r), y, z*cos(r) - x*sin(r));
|
||||
_Point3 rotY(const Scalar r) const {
|
||||
return _Point3(z*sin(r) + x*cos(r), y, z*cos(r) - x*sin(r));
|
||||
}
|
||||
Point3 rotZ(const float r) const {
|
||||
return Point3(x*cos(r) - y*sin(r), x*sin(r) + y*cos(r), z);
|
||||
_Point3 rotZ(const Scalar r) const {
|
||||
return _Point3(x*cos(r) - y*sin(r), x*sin(r) + y*cos(r), z);
|
||||
}
|
||||
Point3 rot(const float rx, const float ry, const float rz) const {
|
||||
_Point3 rot(const Scalar rx, const Scalar ry, const Scalar rz) const {
|
||||
return rotX(rx).rotY(ry).rotZ(rz);
|
||||
//return rotZ(rz).rotY(ry).rotX(rx);
|
||||
}
|
||||
|
||||
|
||||
/** read-only array access */
|
||||
float operator [] (const int idx) const {
|
||||
Scalar operator [] (const int idx) const {
|
||||
Assert::isBetween(idx, 0, 2, "index out of bounds");
|
||||
if (0 == idx) {return x;}
|
||||
if (1 == idx) {return y;}
|
||||
@@ -85,19 +85,19 @@ struct Point3 {
|
||||
}
|
||||
|
||||
/** get the distance between this point and the other one */
|
||||
float getDistance(const Point3& o) const {
|
||||
const float dx = x - o.x;
|
||||
const float dy = y - o.y;
|
||||
const float dz = z - o.z;
|
||||
Scalar getDistance(const _Point3& o) const {
|
||||
const Scalar dx = x - o.x;
|
||||
const Scalar dy = y - o.y;
|
||||
const Scalar dz = z - o.z;
|
||||
return std::sqrt(dx*dx + dy*dy + dz*dz);
|
||||
}
|
||||
|
||||
/** get a normalized copy */
|
||||
Point3 normalized() const {return *this / this->length();}
|
||||
_Point3 normalized() const {return *this / this->length();}
|
||||
|
||||
float length() const {return std::sqrt(x*x + y*y + z*z);}
|
||||
Scalar length() const {return std::sqrt(x*x + y*y + z*z);}
|
||||
|
||||
float length(const float norm) const {
|
||||
Scalar length(const Scalar norm) const {
|
||||
return std::pow(
|
||||
(std::pow(std::abs(x),norm) +
|
||||
std::pow(std::abs(y),norm) +
|
||||
@@ -111,12 +111,15 @@ struct Point3 {
|
||||
|
||||
private:
|
||||
|
||||
static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;}
|
||||
static inline bool ne(const float a, const float b, const float delta) {return std::abs(a-b) > delta;}
|
||||
static inline bool eq(const Scalar a, const Scalar b, const Scalar delta) {return std::abs(a-b) <= delta;}
|
||||
static inline bool ne(const Scalar a, const Scalar b, const Scalar delta) {return std::abs(a-b) > delta;}
|
||||
|
||||
};
|
||||
|
||||
inline float dot(const Point3 p1, const Point3 p2) {
|
||||
//using Point3 = _Point3<double>;
|
||||
using Point3 = _Point3<float>;
|
||||
|
||||
inline double dot(const Point3 p1, const Point3 p2) {
|
||||
return (p1.x*p2.x) + (p1.y*p2.y) + (p1.z*p2.z);
|
||||
}
|
||||
|
||||
@@ -128,4 +131,4 @@ inline Point3 cross(const Point3 a, const Point3 b) {
|
||||
);
|
||||
}
|
||||
|
||||
#endif // GEO_POINT3_H
|
||||
#endif // GEO__Point3_H
|
||||
|
||||
@@ -34,6 +34,18 @@ public:
|
||||
Triangle3 operator - (const Point3 o) const {
|
||||
return Triangle3(p1-o, p2-o, p3-o);
|
||||
}
|
||||
Triangle3& operator += (const Point3 o) {
|
||||
p1 += o;
|
||||
p2 += o;
|
||||
p3 += o;
|
||||
return *this;
|
||||
}
|
||||
Triangle3& operator -= (const Point3 o) {
|
||||
p1 -= o;
|
||||
p2 -= o;
|
||||
p3 -= o;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Point3 getNormal() const {
|
||||
return cross( p2-p1, p3-p1 ).normalized();
|
||||
@@ -47,22 +59,22 @@ public:
|
||||
const Point3 e2 = p3-p1;
|
||||
|
||||
const Point3 h = cross(ray.dir, e2);
|
||||
const float a = dot(e1, h);
|
||||
const double a = dot(e1, h);
|
||||
|
||||
if (a > -0.00001 && a < 0.00001) {return false;}
|
||||
|
||||
const float f = 1/a;
|
||||
const double f = 1/a;
|
||||
|
||||
const Point3 s = ray.start - p1;
|
||||
const float u = f * dot(s,h);
|
||||
const double u = f * dot(s,h);
|
||||
|
||||
if (u < 0.0 || u > 1.0) {return false;}
|
||||
|
||||
const Point3 q = cross(s, e1);
|
||||
const float v = f * dot(ray.dir, q);
|
||||
const double v = f * dot(ray.dir, q);
|
||||
|
||||
if (v < 0.0 || u + v > 1.0) {return false;}
|
||||
const float t = f * dot(e2,q);
|
||||
const double t = f * dot(e2,q);
|
||||
|
||||
|
||||
if (t > 0.00001) {
|
||||
|
||||
@@ -1,9 +1,14 @@
|
||||
#ifndef NAV_MESH_FACTORY_H
|
||||
#define NAV_MESH_FACTORY_H
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "../floorplan/v2/Floorplan.h"
|
||||
#include "../floorplan/v2/FloorplanHelper.h"
|
||||
|
||||
#include "../geo/ConvexHull2.h"
|
||||
#include "../wifi/estimate/ray3/OBJPool.h"
|
||||
|
||||
#include "NavMesh.h"
|
||||
#include "NavMeshTriangle.h"
|
||||
#include "NavMeshFactoryListener.h"
|
||||
@@ -240,10 +245,19 @@ namespace NM {
|
||||
|
||||
// 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) {
|
||||
|
||||
// line-obstacles
|
||||
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
||||
if (line != nullptr) {
|
||||
nmPoly.remove(getPolygon(line));
|
||||
}
|
||||
|
||||
// object-obstacles
|
||||
Floorplan::FloorObstacleObject* obj = dynamic_cast<Floorplan::FloorObstacleObject*>(obs);
|
||||
if (obj != nullptr) {
|
||||
nmPoly.remove(getPolygon(obj));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// construct and add
|
||||
@@ -721,6 +735,23 @@ namespace NM {
|
||||
return res;
|
||||
}
|
||||
|
||||
/** convert the given 3D object to a polygon outline */
|
||||
Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleObject* obj) const {
|
||||
|
||||
Floorplan::Polygon2 res;
|
||||
|
||||
std::vector<Point2> src;
|
||||
Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos);
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
src.push_back(tria.p1.xy());
|
||||
src.push_back(tria.p2.xy());
|
||||
src.push_back(tria.p3.xy());
|
||||
}
|
||||
res.points = ConvexHull2::get(src);
|
||||
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)
|
||||
|
||||
@@ -241,8 +241,8 @@ namespace NM {
|
||||
getUV(p, u, v);
|
||||
|
||||
const Point3 res = getPoint(u,v);
|
||||
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");
|
||||
Assert::isNear<float>(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
|
||||
Assert::isNear<float>(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
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
#define FLOORPLANMESH_H
|
||||
|
||||
#include "Obstacle3.h"
|
||||
#include <fstream>
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
@@ -12,11 +13,35 @@ namespace Ray3D {
|
||||
|
||||
std::vector<Obstacle3D> elements;
|
||||
|
||||
/** export as OBJ file */
|
||||
void exportOBJsimple(const std::string& file) {
|
||||
std::ofstream out(file.c_str());
|
||||
out << toOBJsimple();
|
||||
out.close();
|
||||
}
|
||||
|
||||
/** export as OBJ file */
|
||||
void exportOBJcomplex(const std::string& file, const std::string& nameOnly) {
|
||||
std::ofstream outOBJ((file+".obj").c_str());
|
||||
std::ofstream outMTL((file+".mtl").c_str());
|
||||
OBJData data = toOBJ(nameOnly);
|
||||
outOBJ << data.obj;
|
||||
outMTL << data.mtl;
|
||||
outOBJ.close();
|
||||
outMTL.close();
|
||||
}
|
||||
|
||||
/** export as PLY file */
|
||||
void exportPLY(const std::string& file) {
|
||||
std::ofstream out(file.c_str());
|
||||
out << toPLY();
|
||||
out.close();
|
||||
}
|
||||
|
||||
/** DEBUG: convert to .obj file code for exporting */
|
||||
std::string toOBJ() {
|
||||
std::string toOBJsimple() {
|
||||
|
||||
int nVerts = 1;
|
||||
int nObjs = 0;
|
||||
std::string res;
|
||||
|
||||
// write each obstacle
|
||||
@@ -29,12 +54,87 @@ namespace Ray3D {
|
||||
res += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// write each obstacle
|
||||
for (const Obstacle3D& o : elements) {
|
||||
|
||||
// write the faces
|
||||
for (size_t i = 0; i < o.triangles.size(); ++i) {
|
||||
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
|
||||
nVerts += 3;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// done
|
||||
return res;
|
||||
|
||||
}
|
||||
|
||||
struct OBJData {
|
||||
std::string obj;
|
||||
std::string mtl;
|
||||
};
|
||||
|
||||
/** DEBUG: convert to .obj file code for exporting */
|
||||
OBJData toOBJ(const std::string& name) {
|
||||
|
||||
bool swapYZ = true;
|
||||
int nVerts = 1;
|
||||
int nObjs = 0;
|
||||
OBJData res;
|
||||
|
||||
// write material file
|
||||
for (size_t idx = 0; idx < mats.size(); ++idx) {
|
||||
const Material& mat = mats[idx];
|
||||
res.mtl += "newmtl mat_" + std::to_string(idx) + "\n";
|
||||
res.mtl += "Ka 0.000 0.000 0.000 \n"; // ambient
|
||||
res.mtl += "Kd " + std::to_string(mat.r/255.0f) + " " + std::to_string(mat.g/255.0f) + " " + std::to_string(mat.b/255.0f) + "\n";
|
||||
res.mtl += "Ks 0.000 0.000 0.000 \n";
|
||||
res.mtl += "d " + std::to_string(mat.a/255.0f) + "\n"; // alpha
|
||||
res.mtl += "Tr " + std::to_string(1.0f-mat.a/255.0f) + "\n"; // inv-alpha
|
||||
res.mtl += "illum 2 \n";
|
||||
res.mtl += "\n";
|
||||
}
|
||||
|
||||
|
||||
// use material file
|
||||
res.obj += "mtllib " + name + ".mtl" + "\n";
|
||||
|
||||
// write each obstacle
|
||||
for (const Obstacle3D& o : elements) {
|
||||
|
||||
// write the vertices
|
||||
for (const Triangle3& t : o.triangles) {
|
||||
if (!swapYZ) {
|
||||
res.obj += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.y) + " " + std::to_string(t.p1.z) + "\n";
|
||||
res.obj += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.y) + " " + std::to_string(t.p2.z) + "\n";
|
||||
res.obj += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.y) + " " + std::to_string(t.p3.z) + "\n";
|
||||
} else {
|
||||
res.obj += "v " + std::to_string(t.p1.x) + " " + std::to_string(t.p1.z) + " " + std::to_string(t.p1.y) + "\n";
|
||||
res.obj += "v " + std::to_string(t.p2.x) + " " + std::to_string(t.p2.z) + " " + std::to_string(t.p2.y) + "\n";
|
||||
res.obj += "v " + std::to_string(t.p3.x) + " " + std::to_string(t.p3.z) + " " + std::to_string(t.p3.y) + "\n";
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// write each obstacle
|
||||
for (const Obstacle3D& o : elements) {
|
||||
|
||||
// create a new group
|
||||
res += "g elem_" + std::to_string(++nObjs) + "\n";
|
||||
//res.obj += "g elem_" + std::to_string(++nObjs) + "\n";
|
||||
|
||||
// create a new object
|
||||
res.obj += "o elem_" + std::to_string(++nObjs) + "\n";
|
||||
|
||||
// group's material
|
||||
res.obj += "usemtl mat_" + std::to_string(getMaterial(o)) + "\n";
|
||||
|
||||
// write the group's faces
|
||||
for (size_t i = 0; i < o.triangles.size(); ++i) {
|
||||
res += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
|
||||
res.obj += "f " + std::to_string(nVerts+0) + " " + std::to_string(nVerts+1) + " " + std::to_string(nVerts+2) + "\n";
|
||||
nVerts += 3;
|
||||
}
|
||||
|
||||
@@ -59,18 +159,6 @@ namespace Ray3D {
|
||||
faces += obs.triangles.size();
|
||||
}
|
||||
|
||||
// material
|
||||
std::vector<Material> mats = {
|
||||
|
||||
Material(0,128,0,255), // ground outdoor
|
||||
Material(64,64,64,255), // ground outdoor
|
||||
Material(255,96,96,255), // stair
|
||||
|
||||
Material(128,128,128,255), // concrete
|
||||
Material(64,128,255,64), // glass
|
||||
Material(200,200,200,255), // default
|
||||
|
||||
};
|
||||
|
||||
res << "element material " << mats.size() << "\n";
|
||||
res << "property uchar red\n";
|
||||
@@ -85,11 +173,11 @@ namespace Ray3D {
|
||||
res << "property float nx\n";
|
||||
res << "property float ny\n";
|
||||
res << "property float nz\n";
|
||||
res << "property int material_index\n";
|
||||
res << "property uchar red\n";
|
||||
res << "property uchar green\n";
|
||||
res << "property uchar blue\n";
|
||||
res << "property uchar alpha\n";
|
||||
res << "property int material_index\n";
|
||||
|
||||
res << "element face " << faces << "\n";
|
||||
res << "property list uchar int vertex_indices\n";
|
||||
@@ -107,9 +195,9 @@ namespace Ray3D {
|
||||
const Material& mat = mats[matIdx];
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
const Point3 n = cross(tria.p2-tria.p1, tria.p3-tria.p1).normalized();
|
||||
res << tria.p1.x << " " << tria.p1.y << " " << tria.p1.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << "\n";
|
||||
res << tria.p2.x << " " << tria.p2.y << " " << tria.p2.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
|
||||
res << tria.p3.x << " " << tria.p3.y << " " << tria.p3.z << " " << n.x << " " << n.y << " " << n.z << " " << matIdx << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a <<"\n";
|
||||
res << tria.p1.x << " " << tria.p1.y << " " << tria.p1.z << " " << n.x << " " << n.y << " " << n.z << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << " " << matIdx << "\n";
|
||||
res << tria.p2.x << " " << tria.p2.y << " " << tria.p2.z << " " << n.x << " " << n.y << " " << n.z << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << " " << matIdx << "\n";
|
||||
res << tria.p3.x << " " << tria.p3.y << " " << tria.p3.z << " " << n.x << " " << n.y << " " << n.z << " " << mat.r << " " << mat.g << " " << mat.b << " " << mat.a << " " << matIdx << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
@@ -131,6 +219,20 @@ namespace Ray3D {
|
||||
Material(int r, int g, int b, int a) : r(r), g(g), b(b), a(a) {;}
|
||||
};
|
||||
|
||||
|
||||
// material
|
||||
std::vector<Material> mats = {
|
||||
|
||||
Material(0,128,0,255), // ground outdoor
|
||||
Material(64,64,64,255), // ground outdoor
|
||||
Material(255,96,96,255), // stair
|
||||
|
||||
Material(128,128,128,255), // concrete
|
||||
Material(64,128,255,64), // glass
|
||||
Material(200,200,200,255), // default
|
||||
|
||||
};
|
||||
|
||||
int getMaterial(const Obstacle3D& o) const {
|
||||
if (o.type == Obstacle3D::Type::GROUND_OUTDOOR) {return 0;}
|
||||
if (o.type == Obstacle3D::Type::GROUND_INDOOR) {return 1;}
|
||||
|
||||
@@ -9,6 +9,8 @@
|
||||
#include "Tube.h"
|
||||
#include "FloorplanMesh.h"
|
||||
|
||||
#include "OBJPool.h"
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
/**
|
||||
@@ -25,6 +27,7 @@ namespace Ray3D {
|
||||
bool exportHandrails = true;
|
||||
bool exportDoors = true;
|
||||
bool doorsOpen = true;
|
||||
bool exportObjects = true;
|
||||
bool exportWallTops = false;
|
||||
std::vector<Floorplan::Floor*> exportFloors;
|
||||
|
||||
@@ -114,6 +117,8 @@ namespace Ray3D {
|
||||
/** convert a floor (floor/ceiling) into triangles */
|
||||
std::vector<Obstacle3D> getFloor(const Floorplan::Floor* f) {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
std::vector<Obstacle3D> res;
|
||||
if (!f->enabled) {return res;}
|
||||
if (!f->outline.enabled) {return res;}
|
||||
@@ -157,20 +162,22 @@ namespace Ray3D {
|
||||
Obstacle3D obs(type, Floorplan::Material::CONCRETE);
|
||||
|
||||
// convert them into polygons
|
||||
std::vector<std::vector<Point3>> polys = it.second.get(f->getStartingZ());
|
||||
std::vector<std::vector<Point3>> polys = it.second.get(fpos.z1);
|
||||
|
||||
// convert polygons (GL_TRIANGLE_STRIP) to triangles
|
||||
for (const std::vector<Point3>& pts : polys) {
|
||||
for (int i = 0; i < (int)pts.size() - 2; ++i) {
|
||||
|
||||
// floor must be double-sided for reflection to work with the correct normals
|
||||
// floor must be double-sided
|
||||
Triangle3 tria1 (pts[i+0], pts[i+1], pts[i+2]);
|
||||
Triangle3 tria2 (pts[i+2], pts[i+1], pts[i+0]);
|
||||
|
||||
// ensure the triangle with the normal pointing downwards (towards bulding's cellar)
|
||||
// is below the triangle that points upwards (towards the sky)
|
||||
if (tria1.getNormal().z < 0) {tria1 = tria1 - Point3(0,0,0.02);}
|
||||
if (tria2.getNormal().z < 0) {tria2 = tria2 - Point3(0,0,0.02);}
|
||||
if (tria1.getNormal().z < 0) {std::swap(tria1, tria2);}
|
||||
|
||||
// tria2 = ceiling of previous floor
|
||||
tria2 -= Point3(0,0,fpos.fh);
|
||||
|
||||
// add both
|
||||
obs.triangles.push_back(tria1);
|
||||
@@ -208,6 +215,16 @@ namespace Ray3D {
|
||||
}
|
||||
}
|
||||
|
||||
// handle object obstacles
|
||||
const Floorplan::FloorObstacleObject* foo = dynamic_cast<const Floorplan::FloorObstacleObject*>(fo);
|
||||
if (foo) {
|
||||
if (exportObjects) {
|
||||
if (!foo->file.empty()) {
|
||||
res.push_back(getObject(f, foo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const Floorplan::FloorObstacleDoor* door = dynamic_cast<const Floorplan::FloorObstacleDoor*>(fo);
|
||||
if (door) {
|
||||
if (exportObstacles) {
|
||||
@@ -246,6 +263,8 @@ namespace Ray3D {
|
||||
|
||||
Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
const float thickness_m = fol->thickness_m;
|
||||
const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from);
|
||||
const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to);
|
||||
@@ -255,8 +274,8 @@ namespace Ray3D {
|
||||
const float deg = rad * 180 / M_PI;
|
||||
|
||||
// cube's destination center
|
||||
const float cenZ = (!aboveDoor) ? (f->atHeight + f->height/2) : (f->getEndingZ() - (f->height - aboveDoor->height) / 2);
|
||||
const float height = (!aboveDoor) ? (f->height) : (f->height - aboveDoor->height);
|
||||
const float cenZ = (!aboveDoor) ? (fpos.z1 + fpos.height/2) : (fpos.z2 - (fpos.height - aboveDoor->height) / 2);
|
||||
const float height = (!aboveDoor) ? (fpos.height) : (fpos.height - aboveDoor->height);
|
||||
const Point3 pos(cen2.x, cen2.y, cenZ);
|
||||
|
||||
// div by 2.01 to prevent overlapps and z-fighting
|
||||
@@ -276,8 +295,44 @@ namespace Ray3D {
|
||||
|
||||
}
|
||||
|
||||
/** 3D Obstacle from .obj 3D mesh */
|
||||
Obstacle3D getObject(const Floorplan::Floor* f, const Floorplan::FloorObstacleObject* foo) const {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
const std::string& name = foo->file;
|
||||
Obstacle3D obs = OBJPool::get().getObject(name);
|
||||
obs = obs.rotated_deg( Point3(foo->rot.x, foo->rot.y, foo->rot.z) );
|
||||
obs = obs.translated(foo->pos + Point3(0,0,fpos.z1));
|
||||
|
||||
// std::vector<Triangle3> trias;
|
||||
// for (const OBJReader::Face& face : reader.getData().faces) {
|
||||
// Point3 p1 = face.vnt[0].vertex;
|
||||
// Point3 p2 = face.vnt[1].vertex;
|
||||
// Point3 p3 = face.vnt[2].vertex;
|
||||
// p1 = p1.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p2 = p2.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p3 = p3.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI);
|
||||
// p1 += foo->pos; p1.z += fpos.z1;
|
||||
// p2 += foo->pos; p2.z += fpos.z1;
|
||||
// p3 += foo->pos; p3.z += fpos.z1;
|
||||
// const Triangle3 tria(p1, p2, p3);
|
||||
// trias.push_back(tria);
|
||||
// }
|
||||
|
||||
// // done
|
||||
// Obstacle3D res(Obstacle3D::Type::OBJECT, Floorplan::Material::WOOD);
|
||||
// res.triangles = trias;
|
||||
// return res;
|
||||
|
||||
return obs;
|
||||
|
||||
}
|
||||
|
||||
Obstacle3D getDoor(const Floorplan::Floor* f, const Floorplan::FloorObstacleDoor* door) const {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
const float thickness_m = 0.10; // TODO??
|
||||
const Point2 from = door->from;
|
||||
const Point2 to = door->to;
|
||||
@@ -297,7 +352,7 @@ namespace Ray3D {
|
||||
|
||||
if (doorsOpen) {deg += (door->swap) ? (-90) : (+90);}
|
||||
mat = Matrix4::getTranslation(1,0,0); // cube's edge located at 0,0,0
|
||||
pos = Point3(from.x, from.y, f->atHeight + door->height/2);
|
||||
pos = Point3(from.x, from.y, fpos.z1 + door->height/2);
|
||||
|
||||
const float sx = from.getDistance(to) / 2;
|
||||
const float sy = thickness_m / 2;
|
||||
@@ -312,7 +367,7 @@ namespace Ray3D {
|
||||
} else if (Floorplan::DoorType::REVOLVING == door->type) {
|
||||
|
||||
const Point2 cen2 = (from+to)/2;
|
||||
const Point3 pos(cen2.x, cen2.y, f->atHeight + door->height/2);
|
||||
const Point3 pos(cen2.x, cen2.y, fpos.z1 + door->height/2);
|
||||
|
||||
// outer and inner radius
|
||||
const float rOuter = from.getDistance(to) / 2;
|
||||
@@ -350,11 +405,6 @@ namespace Ray3D {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
return res;
|
||||
|
||||
}
|
||||
@@ -383,6 +433,8 @@ namespace Ray3D {
|
||||
|
||||
Obstacle3D getHandrail(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol) const {
|
||||
|
||||
FloorPos fpos(f);
|
||||
|
||||
// target
|
||||
Obstacle3D res(getType(fol), fol->material);
|
||||
if (!exportHandrails) {return res;}
|
||||
@@ -393,8 +445,8 @@ namespace Ray3D {
|
||||
const Point2 cen2 = (from+to)/2;
|
||||
|
||||
// edges
|
||||
const float z1 = f->atHeight;
|
||||
const float z2 = f->atHeight + 1.0;
|
||||
const float z1 = fpos.z1;
|
||||
const float z2 = fpos.z1 + 1.0;
|
||||
Point3 p1 = Point3(from.x, from.y, z1);
|
||||
Point3 p2 = Point3(to.x, to.y, z1);
|
||||
Point3 p3 = Point3(from.x, from.y, z2);
|
||||
@@ -576,6 +628,15 @@ namespace Ray3D {
|
||||
}
|
||||
}
|
||||
|
||||
/** used to model ceiling thickness */
|
||||
struct FloorPos {
|
||||
float fh;
|
||||
float z1;
|
||||
float z2;
|
||||
float height;
|
||||
FloorPos(const Floorplan::Floor* f) : fh(0.01), z1(f->getStartingZ()), z2(f->getEndingZ()-fh), height(z2-z1) {;}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
108
wifi/estimate/ray3/OBJPool.h
Normal file
108
wifi/estimate/ray3/OBJPool.h
Normal file
@@ -0,0 +1,108 @@
|
||||
#ifndef OBJPOOL_H
|
||||
#define OBJPOOL_H
|
||||
|
||||
#include <vector>
|
||||
#include "../../../geo/Triangle3.h"
|
||||
#include <unordered_map>
|
||||
|
||||
#include "OBJReader.h"
|
||||
#include "Obstacle3.h"
|
||||
|
||||
// LINUX ONLY
|
||||
#include <dirent.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
namespace Ray3D {
|
||||
|
||||
/**
|
||||
* load several named 3D models for quick re-use
|
||||
*/
|
||||
class OBJPool {
|
||||
|
||||
private:
|
||||
|
||||
/** singleton */
|
||||
OBJPool() {;}
|
||||
|
||||
bool initDone = false;
|
||||
std::unordered_map<std::string, Obstacle3D> cache;
|
||||
|
||||
public:
|
||||
|
||||
/** singleton access */
|
||||
static OBJPool& get() {
|
||||
static OBJPool instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
/** data folder */
|
||||
void init(const std::string& folder) {
|
||||
|
||||
initDone = true;
|
||||
|
||||
// LINUX ONLY!
|
||||
DIR* d = opendir(folder.c_str());
|
||||
if (!d) {throw Exception("OBJPool: folder not found: " + folder);}
|
||||
|
||||
struct dirent *dir;
|
||||
while ((dir = readdir(d)) != NULL) {
|
||||
const std::string absFile = folder + "/" + dir->d_name;
|
||||
if (endsWith(absFile, ".obj")) {
|
||||
std::string name = std::string(dir->d_name);
|
||||
name = name.substr(0, name.length() - 4); // without extension
|
||||
load(absFile, name);
|
||||
}
|
||||
}
|
||||
closedir(d);
|
||||
|
||||
}
|
||||
|
||||
/** get all triangles for the given object (if known) */
|
||||
const Obstacle3D& getObject(const std::string& name) {
|
||||
|
||||
// ensure the cache is initialized
|
||||
if (!initDone) {
|
||||
throw Exception("OBJPool: not initialized. call init(folder) first");
|
||||
}
|
||||
|
||||
static Obstacle3D empty;
|
||||
|
||||
// find the entry
|
||||
const auto& it = cache.find(name);
|
||||
if (it == cache.end()) {return empty;}
|
||||
return it->second;
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
inline bool endsWith(std::string const & value, std::string const & ending) {
|
||||
if (ending.size() > value.size()) return false;
|
||||
return std::equal(ending.rbegin(), ending.rend(), value.rbegin());
|
||||
}
|
||||
|
||||
/** load the given .obj file into the cache */
|
||||
void load(const std::string& absName, const std::string& name) {
|
||||
|
||||
OBJReader reader;
|
||||
reader.readFile(absName);
|
||||
//reader.readFile("/mnt/vm/paper/diss/code/IndoorMap/res/mdl/" + file + ".obj"); // todo
|
||||
|
||||
// create triangles
|
||||
Obstacle3D obs;
|
||||
for (const OBJReader::Face& face : reader.getData().faces) {
|
||||
const Triangle3 tria(face.vnt[0].vertex, face.vnt[1].vertex, face.vnt[2].vertex);
|
||||
obs.triangles.push_back(tria);
|
||||
}
|
||||
|
||||
// store
|
||||
cache[name] = obs;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // OBJPOOL_H
|
||||
169
wifi/estimate/ray3/OBJReader.h
Normal file
169
wifi/estimate/ray3/OBJReader.h
Normal file
@@ -0,0 +1,169 @@
|
||||
#ifndef OBJREADER_H
|
||||
#define OBJREADER_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include "../../../geo/Point2.h"
|
||||
#include "../../../geo/Point3.h"
|
||||
|
||||
/**
|
||||
* prase .obj files
|
||||
*/
|
||||
class OBJReader {
|
||||
|
||||
public:
|
||||
|
||||
|
||||
/** group vertex+normal+texture */
|
||||
struct VNT {
|
||||
int idxVertex;
|
||||
int idxNormal;
|
||||
int idxTexture;
|
||||
Point3 vertex;
|
||||
Point3 normal;
|
||||
Point2 texture;
|
||||
};
|
||||
|
||||
/** one triangle */
|
||||
struct Face {
|
||||
VNT vnt[3];
|
||||
Face(VNT v1, VNT v2, VNT v3) : vnt({v1,v2,v3}) {;}
|
||||
};
|
||||
|
||||
/** internal data */
|
||||
struct Data {
|
||||
std::vector<Point3> vertices;
|
||||
std::vector<Point2> texCoords;
|
||||
std::vector<Point3> normals;
|
||||
std::vector<Face> faces;
|
||||
} data;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** ctor. use readXYZ() */
|
||||
OBJReader() {
|
||||
;
|
||||
}
|
||||
|
||||
/** read .obj from the given file */
|
||||
void readFile(const std::string& file) {
|
||||
std::ifstream is(file);
|
||||
std::string line;
|
||||
while(getline(is, line)) {parseLine(line);}
|
||||
is.close();
|
||||
}
|
||||
|
||||
/** read obj from the given data string (.obj file contents) */
|
||||
void readData(const std::string& data) {
|
||||
std::stringstream is(data);
|
||||
std::string line;
|
||||
while(getline(is, line)) {parseLine(line);}
|
||||
}
|
||||
|
||||
/** get the parsed data */
|
||||
const Data& getData() const {return data;}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
template<typename Out>
|
||||
void split(const std::string &s, char delim, Out result) {
|
||||
std::stringstream ss(s);
|
||||
std::string item;
|
||||
while (std::getline(ss, item, delim)) {
|
||||
*(result++) = item;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> split(const std::string &s, char delim) {
|
||||
std::vector<std::string> elems;
|
||||
split(s, delim, std::back_inserter(elems));
|
||||
return elems;
|
||||
}
|
||||
|
||||
/** parse one line of the .obj file */
|
||||
void parseLine(const std::string& line) {
|
||||
|
||||
if (line.length() < 2) {return;}
|
||||
|
||||
const std::vector<std::string> tokens = split(line, ' ');
|
||||
const std::string token = tokens.front();
|
||||
|
||||
if ("v" == token) {parseVertex(tokens);}
|
||||
if ("vt" == token) {parseTexCoord(tokens);}
|
||||
if ("vn" == token) {parseNormal(tokens);}
|
||||
if ("f" == token) {parseFace(tokens);}
|
||||
|
||||
}
|
||||
|
||||
/** parse one vertex from the tokenizer */
|
||||
void parseVertex(const std::vector<std::string>& t) {
|
||||
const float x = std::stof(t[1]);
|
||||
const float y = std::stof(t[2]);
|
||||
const float z = std::stof(t[3]);
|
||||
data.vertices.push_back(Point3(x,y,z));
|
||||
}
|
||||
|
||||
/** parse one texture-coordinate from the tokenizer */
|
||||
void parseTexCoord(const std::vector<std::string>& t) {
|
||||
const float u = std::stof(t[1]);
|
||||
const float v = std::stof(t[2]);
|
||||
data.texCoords.push_back(Point2(u, -v));
|
||||
}
|
||||
|
||||
/** parse one normal from the tokenizer */
|
||||
void parseNormal(const std::vector<std::string>& t) {
|
||||
const float x = std::stof(t[1]);
|
||||
const float y = std::stof(t[2]);
|
||||
const float z = std::stof(t[3]);
|
||||
data.normals.push_back(Point3(x,y,z));
|
||||
}
|
||||
|
||||
/** parse one face from the tokenizer */
|
||||
void parseFace(const std::vector<std::string>& t) {
|
||||
|
||||
std::vector<VNT> indices;
|
||||
|
||||
int numVertices = 0;
|
||||
for (size_t i = 1; i < t.size(); ++i) {
|
||||
|
||||
// one V/T/N
|
||||
const std::string entry = t[i];
|
||||
const std::vector<std::string> vtn = split(entry, '/');
|
||||
|
||||
++numVertices;
|
||||
const std::string v = vtn[0];
|
||||
//const std::string vt = t2.getToken('/', false);
|
||||
//const std::string vn = t2.getToken('/', false);
|
||||
|
||||
// create a new vertex/normal/texture combination
|
||||
VNT vnt;
|
||||
vnt.idxVertex = (std::stoi(v) - 1);
|
||||
//vnt.idxNormal = (vn.empty()) ? (-1) : (std::stoi(vn) - 1);
|
||||
//vnt.idxTexture = (vt.empty()) ? (-1) : (std::stoi(vt) - 1);
|
||||
|
||||
if (vnt.idxVertex >= 0) {vnt.vertex = data.vertices[vnt.idxVertex];}
|
||||
//if (vnt.idxNormal >= 0) {vnt.normal = data.normals[vnt.idxNormal];}
|
||||
//if (vnt.idxTexture >= 0) {vnt.texture = data.texCoords[vnt.idxTexture];}
|
||||
|
||||
indices.push_back(vnt);
|
||||
|
||||
}
|
||||
|
||||
// this will both, create normal triangles and triangulate polygons
|
||||
// see: http://www.mathopenref.com/polygontriangles.html
|
||||
for (int i = 1; i < (int) indices.size()-1; ++i) {
|
||||
Face face(indices[0], indices[1], indices[i+1]);
|
||||
data.faces.push_back(face);
|
||||
}
|
||||
|
||||
// sanity check
|
||||
if (numVertices != 3) {throw "this face is not a triangle!";}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // OBJREADER_H
|
||||
@@ -25,6 +25,7 @@ namespace Ray3D {
|
||||
DOOR,
|
||||
WALL,
|
||||
WINDOW,
|
||||
OBJECT,
|
||||
};
|
||||
|
||||
Type type;
|
||||
@@ -37,6 +38,27 @@ namespace Ray3D {
|
||||
/** ctor */
|
||||
Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
|
||||
|
||||
|
||||
/** translated copy */
|
||||
Obstacle3D translated(const Point3 pos) const {
|
||||
Obstacle3D copy = *this;
|
||||
for (Triangle3& tria : copy.triangles) {
|
||||
tria += pos;
|
||||
}
|
||||
return copy;
|
||||
}
|
||||
|
||||
/** rotated [around (0,0,0)] copy */
|
||||
Obstacle3D rotated_deg(const Point3 rot) const {
|
||||
Obstacle3D copy = *this;
|
||||
for (Triangle3& tria : copy.triangles) {
|
||||
tria.p1 = tria.p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
tria.p2 = tria.p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
tria.p3 = tria.p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI);
|
||||
}
|
||||
return copy;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user