worked on grid-walking

grid/walk/v3/ReachableSampler.h

@@ -0,0 +1,81 @@
+#ifndef INDOOR_GW3_REACHABLESAMPLER_H
+#define INDOOR_GW3_REACHABLESAMPLER_H
+
+#include "../../../math/Random.h"
+
+#include "Reachable.h"
+#include "Helper.h"
+
+namespace GW3 {
+
+	template <typename Node> class ReachableSamplerByDepth {
+
+	public:
+
+		using Entry = typename ReachableByDepthWithDistanceSorted<Node>::Entry;
+
+		struct SampleResult {
+			Point3 pos;
+			float walkDistToStart_m;
+			SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;}
+		};
+
+	private:
+
+		const Grid<Node>& grid;
+		const float gridSize_m;
+
+		const std::vector<Entry>& reachableNodes;
+
+		mutable RandomGenerator gen;
+
+		mutable std::uniform_real_distribution<float> dOffset;
+
+
+	public:
+
+		/** ctor */
+		ReachableSamplerByDepth(const Grid<Node>& grid, const std::vector<Entry>& reachableNodes) :
+			grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) {
+			;
+		}
+
+		SampleResult sample() {
+
+			std::uniform_int_distribution<int> dIdx(0, reachableNodes.size() - 1);
+
+			const int idx = dIdx(gen);
+
+			const Entry* e = &reachableNodes[idx];
+			const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]);
+			const Node* nDst = e->node;
+
+			// center of the destination node
+			const Point3 nodeCenter = Helper<Node>::gpToP3(*nDst);
+
+			// random position within destination-node
+			const float ox = dOffset(gen);
+			const float oy = dOffset(gen);
+
+			// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
+			const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
+
+			// calculate end's walking-distance towards the start
+			float distToStart_m;
+			if (ePrev1) {
+				distToStart_m = ePrev1->walkDistToStart_m + (Helper<Node>::gpToP3(*(ePrev1->node)).getDistance(end));
+			} else {
+				distToStart_m = nodeCenter.getDistance(end);
+			}
+
+			// done
+			return SampleResult(end, distToStart_m);
+
+		}
+
+
+	};
+
+}
+
+#endif // REACHABLESAMPLER_H