worked on 3D model creation
This commit is contained in:
k-a-z-u
@@ -34,478 +34,479 @@
|
||||
// http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
|
||||
|
||||
|
||||
struct Intersection {
|
||||
Point3 pos;
|
||||
const Obstacle3D* obs;
|
||||
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
|
||||
};
|
||||
namespace Ray3D {
|
||||
|
||||
struct StateRay3 : public Ray3 {
|
||||
|
||||
//std::vector<Intersection> stack;
|
||||
|
||||
int depth = 0;
|
||||
float totalLen = 0;
|
||||
float totalAttenuation = 0;
|
||||
|
||||
const Obstacle3D* isWithin = nullptr;
|
||||
|
||||
/** empty ctor */
|
||||
StateRay3() {;}
|
||||
|
||||
/** ctor */
|
||||
StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
|
||||
;
|
||||
}
|
||||
|
||||
StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
StateRay3 next = getNext(hitPos);
|
||||
Assert::isNull(this->isWithin, "code error: isWithin");
|
||||
next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
|
||||
next.isWithin = obs;
|
||||
return next;
|
||||
|
||||
}
|
||||
|
||||
StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
(void) obs;
|
||||
StateRay3 next = getNext(hitPos);
|
||||
next.isWithin = nullptr;
|
||||
return next;
|
||||
|
||||
}
|
||||
|
||||
StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
StateRay3 next = getNext(hitPos);
|
||||
|
||||
next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation;
|
||||
next.isWithin = nullptr; // AIR
|
||||
return next;
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
StateRay3 getNext(const Point3 hitPos) const {
|
||||
StateRay3 next = *this;
|
||||
++next.depth;
|
||||
next.totalLen += (start.getDistance(hitPos));
|
||||
next.start = hitPos;
|
||||
return next;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
inline float getRSSI(const float addDist = 0) const {
|
||||
const float txp = -40;
|
||||
const float gamma = 1.2f;
|
||||
return (txp - 10*gamma*std::log10(totalLen + addDist)) - totalAttenuation;
|
||||
}
|
||||
|
||||
int getDepth() const {
|
||||
//return stack.size();
|
||||
return depth;
|
||||
}
|
||||
|
||||
float getLength() const {
|
||||
return totalLen;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
struct Hit3 {
|
||||
|
||||
const Obstacle3D* obstacle;
|
||||
Triangle3 obstacleTria;
|
||||
|
||||
float dist;
|
||||
Point3 pos;
|
||||
Point3 normal;
|
||||
Floorplan::Material material;
|
||||
bool stopHere = false;
|
||||
bool invalid = false;
|
||||
|
||||
Hit3() {;}
|
||||
Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct Obstacle3DWrapper {
|
||||
|
||||
static std::vector<Point3> getVertices(const Obstacle3D& obs) {
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
pts.push_back(tria.p1);
|
||||
pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p3);
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
static std::vector<Point3> getDebugLines(const Obstacle3D& obs) {
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
pts.push_back(tria.p1); pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p2); pts.push_back(tria.p3);
|
||||
pts.push_back(tria.p3); pts.push_back(tria.p1);
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class WiFiRaytrace3D {
|
||||
|
||||
private:
|
||||
|
||||
BBox3 bbox;
|
||||
Point3 apPos;
|
||||
|
||||
DataMap3Signal dm;
|
||||
|
||||
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;
|
||||
|
||||
struct Limit {
|
||||
static constexpr int RAYS = 15000;
|
||||
static constexpr int HITS = 25;
|
||||
static constexpr float RSSI = -110;
|
||||
struct Intersection {
|
||||
Point3 pos;
|
||||
const Obstacle3D* obs;
|
||||
Intersection(const Point3 pos, const Obstacle3D* obs) : pos(pos), obs(obs) {;}
|
||||
};
|
||||
|
||||
std::vector<Point3> hitEnter;
|
||||
std::vector<Point3> hitLeave;
|
||||
std::vector<Point3> hitStop;
|
||||
struct StateRay3 : public Ray3 {
|
||||
|
||||
public:
|
||||
//std::vector<Intersection> stack;
|
||||
|
||||
/** ctor */
|
||||
WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) {
|
||||
int depth = 0;
|
||||
float totalLen = 0;
|
||||
float totalAttenuation = 0;
|
||||
|
||||
// get the floor's 3D-bbox
|
||||
bbox = FloorplanHelper::getBBox(map);
|
||||
const Obstacle3D* isWithin = nullptr;
|
||||
|
||||
// allocate
|
||||
dm.resize(bbox, gs);
|
||||
/** empty ctor */
|
||||
StateRay3() {;}
|
||||
|
||||
ModelFactory fac(map);
|
||||
std::vector<Obstacle3D> obstacles = fac.triangulize();
|
||||
|
||||
// build bounding volumes
|
||||
for (Obstacle3D& obs : obstacles) {
|
||||
tree.add(obs);
|
||||
/** ctor */
|
||||
StateRay3(const Point3 start, const Point3 dir) : Ray3(start, dir) {
|
||||
;
|
||||
}
|
||||
|
||||
//tree.optimize();
|
||||
//tree.show(500, false);
|
||||
StateRay3 enter(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
int xxx = 0; (void) xxx;
|
||||
|
||||
}
|
||||
|
||||
const std::vector<Point3>& getHitEnter() const {
|
||||
return hitEnter;
|
||||
}
|
||||
const std::vector<Point3>& getHitLeave() const {
|
||||
return hitLeave;
|
||||
}
|
||||
const std::vector<Point3>& getHitStop() const {
|
||||
return hitStop;
|
||||
}
|
||||
|
||||
const DataMap3Signal& estimate() {
|
||||
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<float> dx(-1.0, +1.0);
|
||||
std::uniform_real_distribution<float> dy(-1.0, +1.0);
|
||||
std::uniform_real_distribution<float> dz(-1.0, +1.0);
|
||||
|
||||
#pragma omp parallel for
|
||||
for (int i = 0; i < Limit::RAYS; ++i) {
|
||||
|
||||
std::cout << "ray: " << i << std::endl;
|
||||
|
||||
// direction
|
||||
const Point3 dir = Point3(dx(gen), dy(gen), dz(gen)).normalized();
|
||||
|
||||
// ray
|
||||
const StateRay3 ray(apPos, dir);
|
||||
|
||||
// run!
|
||||
trace(ray);
|
||||
StateRay3 next = getNext(hitPos);
|
||||
Assert::isNull(this->isWithin, "code error: isWithin");
|
||||
next.totalAttenuation += Materials::get().getAttributes(obs->mat).shadowing.attenuation;
|
||||
next.isWithin = obs;
|
||||
return next;
|
||||
|
||||
}
|
||||
|
||||
return dm;
|
||||
StateRay3 leave(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
}
|
||||
(void) obs;
|
||||
StateRay3 next = getNext(hitPos);
|
||||
next.isWithin = nullptr;
|
||||
return next;
|
||||
|
||||
|
||||
//#define USE_DEBUG
|
||||
|
||||
|
||||
private:
|
||||
|
||||
void trace(const StateRay3& ray) {
|
||||
|
||||
// get the nearest intersection with the floorplan
|
||||
const Hit3 nextHit = getNearestHit(ray);
|
||||
|
||||
// stop?
|
||||
if (nextHit.invalid) {
|
||||
#ifdef USE_DEBUG
|
||||
hitStop.push_back(nextHit.pos);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
// rasterize the ray's way onto the map
|
||||
rasterize(ray, nextHit);
|
||||
StateRay3 reflectAt(const Point3 hitPos, const Obstacle3D* obs) const {
|
||||
|
||||
StateRay3 next = getNext(hitPos);
|
||||
|
||||
next.totalAttenuation += Materials::get().getAttributes(obs->mat).reflection.attenuation;
|
||||
next.isWithin = nullptr; // AIR
|
||||
return next;
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
StateRay3 getNext(const Point3 hitPos) const {
|
||||
StateRay3 next = *this;
|
||||
++next.depth;
|
||||
next.totalLen += (start.getDistance(hitPos));
|
||||
next.start = hitPos;
|
||||
return next;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
inline float getRSSI(const float addDist = 0) const {
|
||||
const float txp = -40;
|
||||
const float gamma = 1.2f;
|
||||
return (txp - 10*gamma*std::log10(totalLen + addDist)) - totalAttenuation;
|
||||
}
|
||||
|
||||
int getDepth() const {
|
||||
//return stack.size();
|
||||
return depth;
|
||||
}
|
||||
|
||||
float getLength() const {
|
||||
return totalLen;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
struct Hit3 {
|
||||
|
||||
const Obstacle3D* obstacle;
|
||||
Triangle3 obstacleTria;
|
||||
|
||||
float dist;
|
||||
Point3 pos;
|
||||
Point3 normal;
|
||||
Floorplan::Material material;
|
||||
bool stopHere = false;
|
||||
bool invalid = false;
|
||||
|
||||
Hit3() {;}
|
||||
Hit3(const float dist, const Point3 pos, const Point3 normal) : dist(dist), pos(pos), normal(normal) {;}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct Obstacle3DWrapper {
|
||||
|
||||
static std::vector<Point3> getVertices(const Obstacle3D& obs) {
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
pts.push_back(tria.p1);
|
||||
pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p3);
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
static std::vector<Point3> getDebugLines(const Obstacle3D& obs) {
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
pts.push_back(tria.p1); pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p2); pts.push_back(tria.p3);
|
||||
pts.push_back(tria.p3); pts.push_back(tria.p1);
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class WiFiRaytrace3D {
|
||||
|
||||
private:
|
||||
|
||||
BBox3 bbox;
|
||||
Point3 apPos;
|
||||
|
||||
DataMap3Signal dm;
|
||||
|
||||
BVH3Debug<Obstacle3D, BoundingVolumeSphere3, Obstacle3DWrapper> tree;
|
||||
|
||||
struct Limit {
|
||||
static constexpr int RAYS = 15000;
|
||||
static constexpr int HITS = 25;
|
||||
static constexpr float RSSI = -110;
|
||||
};
|
||||
|
||||
std::vector<Point3> hitEnter;
|
||||
std::vector<Point3> hitLeave;
|
||||
std::vector<Point3> hitStop;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
WiFiRaytrace3D(const Floorplan::IndoorMap* map, const int gs, const Point3 apPos) : apPos(apPos) {
|
||||
|
||||
// get the floor's 3D-bbox
|
||||
bbox = FloorplanHelper::getBBox(map);
|
||||
|
||||
// allocate
|
||||
dm.resize(bbox, gs);
|
||||
|
||||
ModelFactory fac(map);
|
||||
std::vector<Obstacle3D> obstacles = fac.getMesh().elements;
|
||||
|
||||
// build bounding volumes
|
||||
for (Obstacle3D& obs : obstacles) {
|
||||
tree.add(obs);
|
||||
}
|
||||
|
||||
//tree.optimize();
|
||||
//tree.show(500, false);
|
||||
|
||||
int xxx = 0; (void) xxx;
|
||||
|
||||
}
|
||||
|
||||
const std::vector<Point3>& getHitEnter() const {
|
||||
return hitEnter;
|
||||
}
|
||||
const std::vector<Point3>& getHitLeave() const {
|
||||
return hitLeave;
|
||||
}
|
||||
const std::vector<Point3>& getHitStop() const {
|
||||
return hitStop;
|
||||
}
|
||||
|
||||
const DataMap3Signal& estimate() {
|
||||
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<float> dx(-1.0, +1.0);
|
||||
std::uniform_real_distribution<float> dy(-1.0, +1.0);
|
||||
std::uniform_real_distribution<float> dz(-1.0, +1.0);
|
||||
|
||||
#pragma omp parallel for
|
||||
for (int i = 0; i < Limit::RAYS; ++i) {
|
||||
|
||||
std::cout << "ray: " << i << std::endl;
|
||||
|
||||
// direction
|
||||
const Point3 dir = Point3(dx(gen), dy(gen), dz(gen)).normalized();
|
||||
|
||||
// ray
|
||||
const StateRay3 ray(apPos, dir);
|
||||
|
||||
// run!
|
||||
trace(ray);
|
||||
|
||||
}
|
||||
|
||||
return dm;
|
||||
|
||||
// continue?
|
||||
if ((nextHit.stopHere) || (ray.getRSSI(nextHit.dist) < Limit::RSSI) || (ray.getDepth() > Limit::HITS)) {
|
||||
#ifdef USE_DEBUG
|
||||
hitStop.push_back(nextHit.pos);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// apply effects
|
||||
if (ray.isWithin) {
|
||||
leave(ray, nextHit);
|
||||
#ifdef USE_DEBUG
|
||||
hitLeave.push_back(nextHit.pos);
|
||||
#endif
|
||||
} else {
|
||||
enter(ray, nextHit);
|
||||
reflectAt(ray, nextHit);
|
||||
#ifdef USE_DEBUG
|
||||
hitEnter.push_back(nextHit.pos);
|
||||
#endif
|
||||
//#define USE_DEBUG
|
||||
|
||||
|
||||
private:
|
||||
|
||||
void trace(const StateRay3& ray) {
|
||||
|
||||
// get the nearest intersection with the floorplan
|
||||
const Hit3 nextHit = getNearestHit(ray);
|
||||
|
||||
// stop?
|
||||
if (nextHit.invalid) {
|
||||
#ifdef USE_DEBUG
|
||||
hitStop.push_back(nextHit.pos);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
// rasterize the ray's way onto the map
|
||||
rasterize(ray, nextHit);
|
||||
|
||||
// continue?
|
||||
if ((nextHit.stopHere) || (ray.getRSSI(nextHit.dist) < Limit::RSSI) || (ray.getDepth() > Limit::HITS)) {
|
||||
#ifdef USE_DEBUG
|
||||
hitStop.push_back(nextHit.pos);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// apply effects
|
||||
if (ray.isWithin) {
|
||||
leave(ray, nextHit);
|
||||
#ifdef USE_DEBUG
|
||||
hitLeave.push_back(nextHit.pos);
|
||||
#endif
|
||||
} else {
|
||||
enter(ray, nextHit);
|
||||
reflectAt(ray, nextHit);
|
||||
#ifdef USE_DEBUG
|
||||
hitEnter.push_back(nextHit.pos);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
static inline float getAttenuation(const Hit3& h) {
|
||||
return Materials::get().getAttributes(h.material).shadowing.attenuation;
|
||||
}
|
||||
|
||||
static inline float getAttenuationForReflection(const Hit3& h) {
|
||||
return Materials::get().getAttributes(h.material).reflection.attenuation;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/** perform reflection and continue tracing */
|
||||
void leave(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
static inline float getAttenuation(const Hit3& h) {
|
||||
return Materials::get().getAttributes(h.material).shadowing.attenuation;
|
||||
}
|
||||
// continue into the same direction
|
||||
StateRay3 next = ray.leave(hit.pos, hit.obstacle);
|
||||
|
||||
static inline float getAttenuationForReflection(const Hit3& h) {
|
||||
return Materials::get().getAttributes(h.material).reflection.attenuation;
|
||||
}
|
||||
// continue
|
||||
trace(next);
|
||||
|
||||
|
||||
/** perform reflection and continue tracing */
|
||||
void leave(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
// continue into the same direction
|
||||
StateRay3 next = ray.leave(hit.pos, hit.obstacle);
|
||||
|
||||
// continue
|
||||
trace(next);
|
||||
|
||||
}
|
||||
|
||||
/** perform reflection and continue tracing */
|
||||
void enter(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
// continue into the same direction
|
||||
StateRay3 next = ray.enter(hit.pos, hit.obstacle);
|
||||
|
||||
// continue
|
||||
trace(next);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/** perform reflection and continue tracing */
|
||||
void reflectAt(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
if (hit.normal.length() < 0.9) {
|
||||
int i = 0; (void) i;
|
||||
}
|
||||
|
||||
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized");
|
||||
Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized");
|
||||
/** perform reflection and continue tracing */
|
||||
void enter(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
// angle to wide or narrow? -> skip
|
||||
const float d = std::abs(dot(ray.dir, hit.normal));
|
||||
if (d < 0.01) {return;} // parallel
|
||||
if (d > 0.99) {return;} // perpendicular;
|
||||
// continue into the same direction
|
||||
StateRay3 next = ray.enter(hit.pos, hit.obstacle);
|
||||
|
||||
//static std::minstd_rand gen;
|
||||
//static std::normal_distribution<float> dist(0, 0.02);
|
||||
//const float mod = dist(gen);
|
||||
// continue
|
||||
trace(next);
|
||||
|
||||
// reflected ray direction using surface normal
|
||||
const Point3 dir = ray.dir;
|
||||
const Point3 normal = hit.normal;
|
||||
const Point3 refDir = dir - (normal * 2 * dot(dir, normal));
|
||||
}
|
||||
|
||||
// reflected ray;
|
||||
StateRay3 reflected = ray.reflectAt(hit.pos, hit.obstacle);
|
||||
reflected.dir = refDir.normalized();
|
||||
|
||||
// continue
|
||||
trace(reflected);
|
||||
/** perform reflection and continue tracing */
|
||||
void reflectAt(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
}
|
||||
if (hit.normal.length() < 0.9) {
|
||||
int i = 0; (void) i;
|
||||
}
|
||||
|
||||
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "ray's direction is not normalized");
|
||||
Assert::isNear(1.0f, hit.normal.length(), 0.01f, "obstacle's normal is not normalized");
|
||||
|
||||
// angle to wide or narrow? -> skip
|
||||
const float d = std::abs(dot(ray.dir, hit.normal));
|
||||
if (d < 0.01) {return;} // parallel
|
||||
if (d > 0.99) {return;} // perpendicular;
|
||||
|
||||
//static std::minstd_rand gen;
|
||||
//static std::normal_distribution<float> dist(0, 0.02);
|
||||
//const float mod = dist(gen);
|
||||
|
||||
// reflected ray direction using surface normal
|
||||
const Point3 dir = ray.dir;
|
||||
const Point3 normal = hit.normal;
|
||||
const Point3 refDir = dir - (normal * 2 * dot(dir, normal));
|
||||
|
||||
// reflected ray;
|
||||
StateRay3 reflected = ray.reflectAt(hit.pos, hit.obstacle);
|
||||
reflected.dir = refDir.normalized();
|
||||
|
||||
// continue
|
||||
trace(reflected);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) {
|
||||
void hitTest(const StateRay3& ray, const Obstacle3D& obs, Hit3& nearest) {
|
||||
|
||||
const float minDist = 0.01; // prevent errors hitting the same obstacle twice
|
||||
const float minDist = 0.01; // prevent errors hitting the same obstacle twice
|
||||
|
||||
//bool dummy = obs.boundingSphere.intersects(ray);
|
||||
//bool dummy = obs.boundingSphere.intersects(ray);
|
||||
|
||||
Point3 hitPoint;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
if (tria.intersects(ray, hitPoint)) {
|
||||
Point3 hitPoint;
|
||||
for (const Triangle3& tria : obs.triangles) {
|
||||
if (tria.intersects(ray, hitPoint)) {
|
||||
|
||||
//if (!dummy) {
|
||||
// throw Exception("issue!");
|
||||
//}
|
||||
//if (!dummy) {
|
||||
// throw Exception("issue!");
|
||||
//}
|
||||
|
||||
const float dist = hitPoint.getDistance(ray.start);
|
||||
if (dist > minDist && dist < nearest.dist) {
|
||||
nearest.obstacle = &obs;
|
||||
nearest.dist = dist;
|
||||
nearest.pos = hitPoint;
|
||||
nearest.normal = tria.getNormal();
|
||||
nearest.material = obs.mat;
|
||||
const float dist = hitPoint.getDistance(ray.start);
|
||||
if (dist > minDist && dist < nearest.dist) {
|
||||
nearest.obstacle = &obs;
|
||||
nearest.dist = dist;
|
||||
nearest.pos = hitPoint;
|
||||
nearest.normal = tria.getNormal();
|
||||
nearest.material = obs.mat;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Hit3 getNearestHit(const StateRay3& ray) {
|
||||
|
||||
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
|
||||
|
||||
int hits = 0;
|
||||
const float MAX = 999999;
|
||||
Hit3 nearest; nearest.dist = MAX;
|
||||
|
||||
|
||||
// if the ray is currently within something, its only option is to get out of it
|
||||
if (ray.isWithin) {
|
||||
|
||||
// check intersection only with the current obstacle to get out
|
||||
hitTest(ray, *ray.isWithin, nearest);
|
||||
|
||||
} else {
|
||||
|
||||
// // check intersection with all walls
|
||||
// for (const Obstacle3D& obs : obstacles) {
|
||||
|
||||
// // fast opt-out
|
||||
// //if (!obs.boundingSphere.intersects(ray)) {continue;}
|
||||
|
||||
// hitTest(ray, obs, nearest);
|
||||
|
||||
// }
|
||||
|
||||
|
||||
auto onHit = [&] (const Obstacle3D& obs) {
|
||||
++hits;
|
||||
hitTest(ray, obs, nearest);
|
||||
};
|
||||
|
||||
tree.getHits(ray, onHit);
|
||||
|
||||
}
|
||||
|
||||
// no hit with floorplan: limit to bounding-box!
|
||||
if (nearest.dist == MAX) {
|
||||
nearest.invalid = true;
|
||||
}
|
||||
|
||||
//std::cout << hits << std::endl;
|
||||
|
||||
return nearest;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/** rasterize the ray (current pos -> hit) onto the map */
|
||||
void rasterize(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
const Point3 dir = hit.pos - ray.start;
|
||||
const Point3 dirN = dir.normalized();
|
||||
const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f; // TODO * 1.0 ??
|
||||
const int steps = dir.length() / step.length();
|
||||
|
||||
// sanity check
|
||||
// ensure the direction towards the nearest intersection is the same as the ray's direction
|
||||
// otherwise the intersection-test is invalid
|
||||
#ifdef WITH_ASSERTIONS
|
||||
if (dir.normalized().getDistance(ray.dir) > 0.1) {
|
||||
return;
|
||||
std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl;
|
||||
}
|
||||
#endif
|
||||
|
||||
for (int i = 0; i <= steps; ++i) {
|
||||
|
||||
const Point3 dst = ray.start + (step*i);
|
||||
const float partLen = ray.start.getDistance(dst);
|
||||
//const float len = ray.totalLength + partLen;
|
||||
|
||||
// const float curRSSI = dm.get(dst.x, dst.y);
|
||||
const float newRSSI = ray.getRSSI(partLen);
|
||||
const float totalLen = ray.getLength() + partLen;
|
||||
|
||||
// // ray stronger than current rssi?
|
||||
// if (curRSSI == 0 || curRSSI < newRSSI) {
|
||||
// dm.set(dst.x, dst.y, newRSSI);
|
||||
// }
|
||||
|
||||
dm.update(dst.x, dst.y, dst.z, newRSSI, totalLen);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Hit3 getNearestHit(const StateRay3& ray) {
|
||||
|
||||
Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
|
||||
|
||||
int hits = 0;
|
||||
const float MAX = 999999;
|
||||
Hit3 nearest; nearest.dist = MAX;
|
||||
|
||||
|
||||
/*
|
||||
Sphere3 getSphereAround(const std::vector<Triangle3>& trias) {
|
||||
// if the ray is currently within something, its only option is to get out of it
|
||||
if (ray.isWithin) {
|
||||
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : trias) {
|
||||
pts.push_back(tria.p1);
|
||||
pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p3);
|
||||
}
|
||||
const Sphere3 sphere = Sphere3::around(pts);
|
||||
// check intersection only with the current obstacle to get out
|
||||
hitTest(ray, *ray.isWithin, nearest);
|
||||
|
||||
} else {
|
||||
|
||||
// // check intersection with all walls
|
||||
// for (const Obstacle3D& obs : obstacles) {
|
||||
|
||||
// // fast opt-out
|
||||
// //if (!obs.boundingSphere.intersects(ray)) {continue;}
|
||||
|
||||
// hitTest(ray, obs, nearest);
|
||||
|
||||
// }
|
||||
|
||||
|
||||
auto onHit = [&] (const Obstacle3D& obs) {
|
||||
++hits;
|
||||
hitTest(ray, obs, nearest);
|
||||
};
|
||||
|
||||
tree.getHits(ray, onHit);
|
||||
|
||||
}
|
||||
|
||||
// no hit with floorplan: limit to bounding-box!
|
||||
if (nearest.dist == MAX) {
|
||||
nearest.invalid = true;
|
||||
}
|
||||
|
||||
//std::cout << hits << std::endl;
|
||||
|
||||
return nearest;
|
||||
|
||||
// sanity assertion
|
||||
for (const Point3& pt : pts) {
|
||||
Assert::isTrue(sphere.contains(pt), "bounding sphere error");
|
||||
}
|
||||
|
||||
return sphere;
|
||||
|
||||
}
|
||||
*/
|
||||
/** rasterize the ray (current pos -> hit) onto the map */
|
||||
void rasterize(const StateRay3& ray, const Hit3& hit) {
|
||||
|
||||
const Point3 dir = hit.pos - ray.start;
|
||||
const Point3 dirN = dir.normalized();
|
||||
const Point3 step = dirN * (dm.getGridSize_cm()/100.0f) * 1.0f; // TODO * 1.0 ??
|
||||
const int steps = dir.length() / step.length();
|
||||
|
||||
// sanity check
|
||||
// ensure the direction towards the nearest intersection is the same as the ray's direction
|
||||
// otherwise the intersection-test is invalid
|
||||
#ifdef WITH_ASSERTIONS
|
||||
if (dir.normalized().getDistance(ray.dir) > 0.1) {
|
||||
return;
|
||||
std::cout << "direction to the nearest hit is not the same direction as the ray has. incorrect intersection test?!" << std::endl;
|
||||
}
|
||||
#endif
|
||||
|
||||
for (int i = 0; i <= steps; ++i) {
|
||||
|
||||
const Point3 dst = ray.start + (step*i);
|
||||
const float partLen = ray.start.getDistance(dst);
|
||||
//const float len = ray.totalLength + partLen;
|
||||
|
||||
// const float curRSSI = dm.get(dst.x, dst.y);
|
||||
const float newRSSI = ray.getRSSI(partLen);
|
||||
const float totalLen = ray.getLength() + partLen;
|
||||
|
||||
// // ray stronger than current rssi?
|
||||
// if (curRSSI == 0 || curRSSI < newRSSI) {
|
||||
// dm.set(dst.x, dst.y, newRSSI);
|
||||
// }
|
||||
|
||||
dm.update(dst.x, dst.y, dst.z, newRSSI, totalLen);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
/*
|
||||
Sphere3 getSphereAround(const std::vector<Triangle3>& trias) {
|
||||
|
||||
std::vector<Point3> pts;
|
||||
for (const Triangle3& tria : trias) {
|
||||
pts.push_back(tria.p1);
|
||||
pts.push_back(tria.p2);
|
||||
pts.push_back(tria.p3);
|
||||
}
|
||||
const Sphere3 sphere = Sphere3::around(pts);
|
||||
|
||||
// sanity assertion
|
||||
for (const Point3& pt : pts) {
|
||||
Assert::isTrue(sphere.contains(pt), "bounding sphere error");
|
||||
}
|
||||
|
||||
return sphere;
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // WIFIRAYTRACE3D_H
|
||||
|
||||
Reference in New Issue
Block a user