Indoor/navMesh/walk/NavMeshWalkSemiRandom.h

#ifndef NAVMESHWALKSEMIRANDOM_H
#define NAVMESHWALKSEMIRANDOM_H

#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"

#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"


namespace NM {

	/**
	 * similar to NavMeshWalkRandom but:
	 * pick a semi random destination within the reachable area (requested distance/heading + strong deviation)
	 * if this destination is reachable:
	 * weight this area (evaluators)
	 * repeat this some times to find a robus destination
	 */
	template <typename Tria> class NavMeshWalkSemiRandom {

	private:

		const NavMesh<Tria>& mesh;

		std::vector<NavMeshWalkEval<Tria>*> evals;

	public:


		struct ResultEntry {
			NavMeshLocation<Tria> location;
			Heading heading;
			double probability;
			ResultEntry() : heading(0) {;}
		};

		struct ResultList : public std::vector<ResultEntry> {};

	public:

		/** ctor */
		NavMeshWalkSemiRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {

		}

		/** add a new evaluator to the walker */
		void addEvaluator(NavMeshWalkEval<Tria>* eval) {
			this->evals.push_back(eval);
		}

		ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {

			static Distribution::Normal<float> dDist(1.0, 0.4);
			static Distribution::Normal<float> dHead(0.0, 1.0);

			// construct reachable region
			const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
			const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);

			ResultEntry re;

			NavMeshPotentialWalk<Tria> pwalk(params);
			pwalk.end = reachable.getRandom().draw();	// to have at least a non-start solution
			re.probability = eval(pwalk);
			re.location = pwalk.end;


			for (int i = 0; i < 10; ++i) {

				const float distance = params.getToBeWalkedDistance() * dDist.draw();
				const Heading head = params.heading + dHead.draw();

				// only forward!
				if (distance < 0.01) {--i; continue;}

				// get the to-be-reached destination's position (using start+distance+heading)
				const Point2 dir = head.asVector();
				const Point2 dst = params.start.pos.xy() + (dir * distance);

				const Tria* dstTria = reachable.getContainingTriangle(dst);

				// is above destination reachable?
				if (dstTria) {

					pwalk.end.pos = dstTria->toPoint3(dst);
					pwalk.end.tria = dstTria;
					const double p = eval(pwalk);

					// better?
					if (p > re.probability) {
						re.location = pwalk.end;
						re.probability = p;
						re.heading = head;
					}

				}

			}

			return re;

		}

		ResultList getMany(const NavMeshWalkParams<Tria>& params) const {

			static Distribution::Normal<float> dDist(1.0, 0.4);
			static Distribution::Normal<float> dHead(0.0, 1.0);

			ResultList res;

			// construct reachable region
			const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
			const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);

			NavMeshPotentialWalk<Tria> pwalk(params);

			for (int i = 0; i < 25; ++i) {

				const float distance = params.getToBeWalkedDistance() * dDist.draw();
				const Heading head = params.heading + dHead.draw();

				// only forward!
				if (distance < 0.01) {--i; continue;}

				// get the to-be-reached destination's position (using start+distance+heading)
				const Point2 dir = head.asVector();
				const Point2 dst = params.start.pos.xy() + (dir * distance);

				const Tria* dstTria = reachable.getContainingTriangle(dst);

				// is above destination reachable?
				if (dstTria) {

					pwalk.end.pos = dstTria->toPoint3(dst);
					pwalk.end.tria = dstTria;
					const double p = eval(pwalk);

					ResultEntry re;
					re.location = pwalk.end;
					re.probability = p;
					re.heading = head;
					res.push_back(re);

				}

			}

			return res;

		}

		double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
			double p = 1.0;
			for (const NavMeshWalkEval<Tria>* eval : evals) {
				const double p1 = eval->getProbability(pwalk);
				p *= p1;
			}
			return p;
		}


	};

}

#endif // NAVMESHWALKSEMIRANDOM_H