work on raytracing

wifi/estimate/ray2d/DataMap2.h

@@ -0,0 +1,244 @@
+#ifndef DATAMAP2_H
+#define DATAMAP2_H
+
+#include "../../../geo/BBox2.h"
+#include <vector>
+
+template <typename T> class DataMap {
+
+private:
+
+	float sx_m;
+	float sy_m;
+
+	float x_m;
+	float y_m;
+	int gridSize_cm;
+
+	BBox2 bbox;
+
+	int nx;
+	int ny;
+
+	T* data = nullptr;
+
+public:
+
+	/** ctor */
+	DataMap() {
+		;
+	}
+
+	~DataMap() {
+
+		// cleanup
+		cleanup();
+
+	}
+
+	DataMap(const DataMap&) = delete;
+	DataMap* operator = (const DataMap& o) = delete;
+
+	/*
+	void blured(DataMap<T>& dst) const {
+
+		const int s = 2;
+		dst.resize(this->bbox, this->gridSize_cm);
+
+		for (int iy = 0; iy < ny; ++iy) {
+			for (int ix = 0; ix < nx; ++ix) {
+
+				float valSum = 0;
+				int cntSum = 0;
+
+				for (int oy = -s; oy <= +s; ++oy) {
+					for (int ox = -s; ox <= +s; ++ox) {
+
+						const int x = ix+ox;
+						const int y = iy+oy;
+
+						if (containsGrid(x,y)) {
+							valSum += getGrid(x,y);
+							++cntSum;
+						}
+
+					}
+				}
+
+				dst.setGrid(ix, iy, valSum/cntSum);
+
+			}
+		}
+
+	}
+	*/
+
+	/** does the map contain the given indices? */
+	bool containsGrid(const int x, const int y) const {
+		return (x >= 0) && (y >= 0) && (x < nx) && (y < ny);
+	}
+
+	/** does the map contain the given coordinate? */
+	bool contain(const float x_m, const float y_m) const {
+		return bbox.contains(Point2(x_m, y_m));
+	}
+
+	void resize(const BBox2 bbox, const int gridSize_cm) {
+
+		// cleanup
+		cleanup();
+
+		this->bbox = bbox;
+
+		// slightly increase to pervent out-of-bounds due to rounding
+		float buffer_m = 1;
+
+		// start-offset
+		sx_m = bbox.getMin().x - buffer_m;
+		sy_m = bbox.getMin().y - buffer_m;
+
+		// size in meter
+		x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
+		y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
+
+		// number of elements in the grid
+		this->gridSize_cm = gridSize_cm;
+		nx = (x_m*100) / gridSize_cm;
+		ny = (y_m*100) / gridSize_cm;
+
+		// allocate and reset all to 0.0
+		data = new T[nx*ny];
+		//std::fill(&data[0], &data[nx*ny], 0);
+
+	}
+
+	/** get the used grid-size (in cm) */
+	int getGridSize_cm() const {return gridSize_cm;}
+
+	void set(const float x_m, const float y_m, const T val) {
+		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
+		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
+		setGrid(ix, iy, val);
+	}
+
+	T get(const float x_m, const float y_m) const {
+		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
+		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
+		return getGrid(ix, iy);
+	}
+
+	T& getRef(const float x_m, const float y_m) {
+		const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
+		const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
+		return getGridRef(ix, iy);
+	}
+
+	T getGrid(const int ix, const int iy) const {
+		Assert::isBetween(ix, 0, nx-1, "x out of range");
+		Assert::isBetween(iy, 0, ny-1, "y out of range");
+		const int idx = ix + iy*nx;
+		return data[idx];
+	}
+
+	T& getGridRef(const int ix, const int iy) {
+		Assert::isBetween(ix, 0, nx-1, "x out of range");
+		Assert::isBetween(iy, 0, ny-1, "y out of range");
+		const int idx = ix + iy*nx;
+		return data[idx];
+	}
+
+	void setGrid(const int ix, const int iy, const T val) {
+		Assert::isBetween(ix, 0, nx-1, "x out of range");
+		Assert::isBetween(iy, 0, ny-1, "y out of range");
+		const int idx = ix + iy*nx;
+		data[idx] = val;
+	}
+
+	void forEach(std::function<void(float,float,T)> func) const {
+		for (int iy = 0; iy < ny; ++iy) {
+			for (int ix = 0; ix < nx; ++ix) {
+				const float x = (ix * gridSize_cm / 100.0f) + sx_m;
+				const float y = (iy * gridSize_cm / 100.0f) + sy_m;
+				func(x,y,getGrid(ix, iy));
+			}
+		}
+	}
+
+	/*
+	void dump() {
+
+		std::ofstream os("/tmp/1.dat");
+		const float s = 1;//gridSize_cm / 100.0f;
+//		for (int y = 0; y < ny; ++y) {
+//			for (int x = 0; x < nx; ++x) {
+//				float rssi = data[x+y*nx];
+//				rssi = (rssi == 0) ? (-100) : (rssi);
+//				os << (x*s) << " " << (y*s) << " " << rssi << "\n";
+//			}
+//			os << "\n";
+//		}
+		for (int y = 0; y < ny; ++y) {
+			for (int x = 0; x < nx; ++x) {
+				float rssi = data[x+y*nx];
+				rssi = (rssi == 0) ? (-100) : (rssi);
+				os << rssi << " ";
+			}
+			os << "\n";
+		}
+		os.close();
+
+	}
+	*/
+
+private:
+
+	void cleanup() {
+		delete[] data;
+		data = nullptr;
+	}
+
+
+};
+
+
+
+struct DataMapSignalEntry {
+
+	struct Entry {
+		float rssi;
+		float distanceToAP;
+		Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
+	};
+
+	std::vector<Entry> entries;
+
+	void add(const float rssi, const float distanceToAP) {
+		Entry e(rssi, distanceToAP);
+		entries.push_back(e);
+	}
+
+	float getMaxRSSI() const {
+		auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
+		if (entries.empty()) {return -120;}
+		auto it = std::max_element(entries.begin(), entries.end(), comp);
+		return it->rssi;
+	}
+
+
+};
+
+class DataMapSignal : public DataMap<DataMapSignalEntry> {
+
+public:
+
+	/** update average */
+	void update(const float x_m, const float y_m, const float rssi, const float distanceToAP) {
+
+		DataMapSignalEntry& entry = getRef(x_m, y_m);
+		entry.add(rssi, distanceToAP);
+
+	}
+
+};
+
+#endif // DATAMAP2_H

wifi/estimate/ray2d/MaterialOptions.h

@@ -0,0 +1,63 @@
+#ifndef MATERIALOPTIONS_H
+#define MATERIALOPTIONS_H
+
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../Assertions.h"
+
+/** raytracing attributes for one material */
+struct MaterialAttributes {
+
+	struct Shadowing {
+		float attenuation;
+		Shadowing() : attenuation(NAN) {;}
+		Shadowing(float attenuation) : attenuation(attenuation) {;}
+	} shadowing;
+
+	struct Reflection {
+		float attenuation;
+		Reflection() : attenuation(NAN) {;}
+		Reflection(float attenuation) : attenuation(attenuation) {;}
+	} reflection;
+
+	MaterialAttributes(float shadowA, float reflectA) : shadowing(shadowA), reflection(reflectA) {;}
+
+	MaterialAttributes() {;}
+
+};
+
+class Materials {
+
+public:
+
+	/** singleton access */
+	static Materials& get() {
+		static Materials instance;
+		return instance;
+	}
+
+	/** get the attributes for the given material */
+	inline const MaterialAttributes& getAttributes(const Floorplan::Material mat) const {
+		const int idx = (const int) mat;
+		Assert::isBetween(idx, 0, (int)materials.size()-1, "material index out of bounds");
+		return materials[idx];
+	}
+
+private:
+
+	std::vector<MaterialAttributes> materials;
+
+	/** hidden ctor */
+	Materials() {
+
+		materials.resize((int)Floorplan::Material::_END);
+		materials[(int)Floorplan::Material::CONCRETE] = MaterialAttributes(12, 5);
+		materials[(int)Floorplan::Material::DRYWALL] = MaterialAttributes(2, 5);
+		materials[(int)Floorplan::Material::GLASS] = MaterialAttributes(28, 2);
+		materials[(int)Floorplan::Material::UNKNOWN] = MaterialAttributes(2, 5);
+		materials[(int)Floorplan::Material::WOOD] = MaterialAttributes(5, 5);
+
+	}
+
+};
+
+#endif // MATERIALOPTIONS_H

wifi/estimate/ray2d/Ray2.h

@@ -1,4 +1,24 @@
 #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

wifi/estimate/ray2d/WiFiRayTrace2D.h

@@ -1,4 +1,314 @@
 #ifndef WIFIRAYTRACE2D_H
 #define WIFIRAYTRACE2D_H
 
+#include "../../../geo/Point2.h"
+#include "../../../geo/Line2.h"
+#include "../../../geo/BBox2.h"
+
+#include "../../../floorplan/v2/Floorplan.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+
+#include "DataMap2.h"
+#include "Ray2.h"
+#include "MaterialOptions.h"
+
+#include <random>
+
+// http://www.realtimerendering.com/resources/RTNews/html/rtnv10n1.html#art3
+// http://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector
+
+// RADIO WAVES
+// http://people.seas.harvard.edu/%7Ejones/es151/prop_models/propagation.html			Indoor Wireless RF Channel
+// http://www.electronics-radio.com/articles/antennas-propagation/propagation-overview/radio-em-wave-reflection.php
+
+
+
+
+struct StateRay2 : public Ray2 {
+
+	/** already travelled distance from the AP (by all previous rays */
+	float totalLength;
+
+	/** attenuation taken since the start */
+	float totalAttenuation;
+
+	void* lastObstacle;
+	int depth = 0;
+
+	/** empty ctor */
+	StateRay2() : totalLength(NAN), totalAttenuation(NAN) {;}
+
+	/** ctor */
+	StateRay2(const Point2 start, const Point2 dir) : Ray2(start, dir), totalLength(0), totalAttenuation(0) {
+		;
+	}
+
+	inline float getRSSI(const float addDist = 0) const {
+		const float txp = -40;
+		const float gamma = 1.2f;
+		return (txp - 10*gamma*std::log10(totalLength + addDist)) - totalAttenuation;
+	}
+
+};
+
+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 {
+
+private:
+
+	const Floorplan::Floor* floor;
+	BBox2 bbox;
+	Point2 apPos;
+
+	DataMapSignal dm;
+
+	struct Limit {
+		static constexpr int RAYS = 2000;
+		static constexpr int HITS = 11;
+		static constexpr float RSSI = -110;
+	};
+
+public:
+
+	/** ctor */
+	WiFiRaytrace2D(const Floorplan::Floor* floor, const int gs, const Point2 apPos) : floor(floor), apPos(apPos) {
+
+		// get the floor's 3D-bbox
+		BBox3 bb3 = FloorplanHelper::getBBox(floor);
+
+		// 2D only party
+		bbox = BBox2(bb3.getMin().xy(), bb3.getMax().xy());
+
+		// allocate
+		dm.resize(bbox, gs);
+
+	}
+
+	const DataMapSignal& estimate() {
+
+		for (int i = 0; i < Limit::RAYS; ++i) {
+
+			std::cout << "ray: " << i << std::endl;
+
+			// angle between starting-rays
+			const float angle = (float)M_PI*2.0f * i / Limit::RAYS;
+
+			// direction
+			const Point2 dir(std::cos(angle), std::sin(angle));
+
+			// ray
+			const StateRay2 ray(apPos, dir);
+
+			// run!
+			trace(ray);
+
+		}
+
+		return dm;
+
+	}
+
+
+
+private:
+
+	static float dot(const Point2 p1, const Point2 p2) {
+		return (p1.x * p2.x) + (p1.y * p2.y);
+	}
+
+	void trace(const StateRay2& ray) {
+
+
+		// get the nearest intersection with the floorplan
+		const Hit hit = getNearestHit(ray);
+
+		// rasterize the ray's way onto the map
+		rasterize(ray, hit);
+
+		// 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);
+		shadowed(ray, hit);
+
+
+	}
+
+	static inline float getAttenuation(const Hit& h) {
+		return Materials::get().getAttributes(h.material).shadowing.attenuation;
+	}
+
+	static inline float getAttenuationForReflection(const Hit& h) {
+		return Materials::get().getAttributes(h.material).reflection.attenuation;
+	}
+
+	/** perform reflection and continue tracing */
+	void shadowed(const StateRay2& ray, const Hit& hit) {
+
+		StateRay2 next = ray;							// continue into the same direction
+		next.depth = ray.depth + 1;
+		next.lastObstacle = hit.obstacle;
+		next.start = hit.pos;
+		next.totalLength += hit.dist;
+		next.totalAttenuation += getAttenuation(hit);	// contribute attenuation
+
+		trace(next);
+
+	}
+
+	/** perform reflection and continue tracing */
+	void reflected(const StateRay2& ray, const Hit& hit) {
+
+		Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized");
+		Assert::isNear(1.0f, hit.normal.length(), 0.01f, "not normalized");
+
+		// angle to wide or narrow? -> skip
+		const float d = std::abs(dot(ray.dir, hit.normal));
+		if (d < 0.05) {return;}		// parallel
+		if (d > 0.95) {return;}		// perpendicular;
+
+		static std::minstd_rand gen;
+		static std::normal_distribution<float> dist(0, 0.02);
+		const float mod = dist(gen);
+		Point2 normalMod(
+			hit.normal.x * std::cos(mod) - hit.normal.y * std::sin(mod),
+			hit.normal.y * std::cos(mod) - hit.normal.x * std::sin(mod)
+		);
+
+
+		// reflected ray;
+		StateRay2 reflected;
+		reflected.depth = ray.depth + 1;
+		reflected.lastObstacle = hit.obstacle;
+		reflected.start = hit.pos;
+		reflected.totalLength = ray.totalLength + hit.dist;
+		reflected.totalAttenuation = ray.totalAttenuation + getAttenuationForReflection(hit);		// TODO
+		reflected.dir = (ray.dir - normalMod * (dot(ray.dir, hit.normal)) * 2).normalized();		// slight variation
+
+		trace(reflected);
+
+	}
+
+
+
+	Hit getNearestHit(const StateRay2& ray) {
+
+		Assert::isNear(1.0f, ray.dir.length(), 0.01f, "not normalized!");
+
+		const Line2 longRay(ray.start, ray.start + ray.dir*100);
+
+		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) {
+
+			// do not hit the last obstacle again
+			if (ray.lastObstacle == fo) {continue;}
+
+			// get the line
+			const Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(fo);
+			const Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(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;
+				}
+			}
+		}
+
+		// no hit with floorplan: limit to bounding-box!
+		if (nearest.dist == MAX) {
+			//const BBox2 bb( Point2(0,0), Point2(w, h) );
+			Point2 hit;
+			for (Line2 l : bbox.lines()) {
+				if (l.getSegmentIntersection(longRay, hit)) {
+					nearest.obstacle = nullptr;
+					nearest.dist = hit.getDistance(ray.start);
+					nearest.pos = hit;
+					nearest.normal = (l.p2 - l.p1).perpendicular().normalized();
+					nearest.material = Floorplan::Material::UNKNOWN;
+					nearest.stopHere = true;
+				}
+			}
+		}
+
+		return nearest;
+
+	}
+
+	/** rasterize the ray (current pos -> hit) onto the map */
+	void rasterize(const StateRay2& ray, const Hit& hit) {
+
+		const Point2 dir = hit.pos - ray.start;
+		const Point2 dirN = dir.normalized();
+		const Point2 step = dirN * (dm.getGridSize_cm()/100.0f) * 0.7f;		// 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 Point2 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.totalLength + partLen;
+
+//			// ray stronger than current rssi?
+//			if (curRSSI == 0 || curRSSI < newRSSI) {
+//				dm.set(dst.x, dst.y, newRSSI);
+//			}
+
+			dm.update(dst.x, dst.y, newRSSI, totalLen);
+
+		}
+
+
+	}
+
+
+
+
+};
+
 #endif // WIFIRAYTRACE2D_H