worked on raytracing

wifi/estimate/ray2d/WiFiRayTrace2D.h

@@ -141,14 +141,97 @@ public:
 //			usleep(1000*100);
 //		}
 
+
+
 //		tree.show();
 		tree.optimize(250);
 //		int depth = tree.getDepth();
 		tree.show(1500,false);
 
+		constructNeighbors(dm);
+		showNeighbors(dm);
+
+		int i = 0;
+
 
 	}
 
+public:
+
+	void showNeighbors(DataMapSignal& map) {
+
+		static K::Gnuplot gp;
+		K::GnuplotPlot plot;
+		K::GnuplotPlotElementLines lines; plot.add(&lines);
+
+		auto func = [&] (const int ix, const int iy, const DataMapSignalEntry& e) {
+
+			const Point2 p1 = map.gridToPos(ix, iy);
+
+			for (const int idx : e.neighbors) {
+				const Point2 p2 = map.idxToPos(idx);
+
+				K::GnuplotPoint2 gp1(p1.x, p1.y);
+				K::GnuplotPoint2 gp2(p2.x, p2.y);
+				lines.addSegment(gp1, gp2);
+
+			}
+
+		};
+
+		map.forEachGrid(func);
+
+		gp.draw(plot);
+		gp.flush();
+
+		int i = 0;
+
+
+	}
+
+	/** construct neighborship relations between nodes not intersected by walls */
+	void constructNeighbors(DataMapSignal& map) {
+
+		auto func = [&] (const int ix, const int iy, DataMapSignalEntry& e) {
+			for (int dy = -1; dy <= +1; ++dy) {
+				for (int dx = -1; dx <= +1; ++dx) {
+
+					// x/y index for the potential neighbor
+					const int ix2 = ix+dx;
+					const int iy2 = iy+dy;
+
+					// out of bounds?
+					if (!map.containsGrid(ix2,iy2)) {continue;}
+
+					// intersection test
+					const Point2 p1 = map.gridToPos(ix, iy);
+					const Point2 p2 = map.gridToPos(ix2, iy2);
+					const Line2 line(p1,p2);
+					const Point2 dir = (p2-p1).normalized();
+					const Ray2 ray(p1, dir);
+
+					bool isConnectable = true;
+					auto onHit = [&] (const Obstacle2D& obs) {
+
+						if (obs.line.getSegmentIntersection(line)) {isConnectable = false;}
+
+					};
+
+					tree.getHits(ray, onHit);
+
+					if (isConnectable) {
+						e.neighbors.push_back(map.getIndex(ix2, iy2));
+					}
+
+				}
+			}
+		};
+
+		map.forEachGrid(func);
+
+	}
+
+
 	const DataMapSignal& estimate() {
 
 		for (int i = 0; i < Limit::RAYS; ++i) {
@@ -257,7 +340,11 @@ private:
 	}
 
 
-	static inline void hitTest(const Line2& longRay, const Obstacle2D& obs, Hit& nearest) {
+	static inline double crossVal(const Point2 v, const Point2 w) {
+		return ((double)v.x*(double)w.y) - ((double)v.y*(double)w.x);
+	}
+
+	static inline void hitTest(const Ray2& ray, const Obstacle2D& obs, Hit& nearest) {
 
 		const float minDist = 0.01;		// prevent errors hitting the same obstacle twice
 
@@ -266,8 +353,8 @@ private:
 
 		// get the line
 		Point2 hit;
-		if (obs.line.getSegmentIntersection(longRay, hit)) {
-			const float dist = hit.getDistance(longRay.p1);
+		if ( obs.line.intersects(ray, hit) ) {			// TODO rounding issues?!
+			const float dist = hit.getDistance(ray.start);
 			if (dist > minDist && dist < nearest.dist) {
 				nearest.obstacle = &obs;
 				nearest.dist = dist;
@@ -291,9 +378,10 @@ private:
 
 		//int hits = 0;
 
-		const auto onHit = [longRay, &nearest] (const Obstacle2D& obs) {
+		const auto onHit = [ray, &nearest] (const Obstacle2D& obs) {
 			//++hits;
-			hitTest(longRay, obs, nearest);
+			//hitTest(longRay, obs, nearest);
+			hitTest(ray, obs, nearest);
 		};
 
 		tree.getHits(ray, onHit);