Indoor/navMesh/walk/NavMeshWalkEval.h

/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef NAVMESHWALKEVAL_H
#define NAVMESHWALKEVAL_H

#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/distribution/Normal.h"
#include "../../misc/PerfCheck.h"
#include "../../Assertions.h"
#include "../meta/NavMeshDijkstra.h"


namespace NM {

	template <typename Tria> struct NavMeshPotentialWalk {

		NavMeshWalkParams<Tria> requested;

		NavMeshLocation<Tria> end;

		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
			;
		}

		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
			;
		}

	};

	/**
	 * evaluate a NavMeshWalk from -> to = probability
	 */
	template <typename Tria> class NavMeshWalkEval {

	public:

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;

	};







//	/**
//	 * evaluate the difference between head(start,end) and the requested heading
//	 */
//	template <typename Tria> class WalkEvalHeadingStartEndVonMises : public NavMeshWalkEval<Tria> {

//		const double sigma_rad;
//		const double kappa;
//		Distribution::VonMises<double> _dist;
//		Distribution::LUT<double> dist;

//	public:

//		// kappa = 1/var = 1/sigma^2
//		// https://en.wikipedia.org/wiki/Von_Mises_distribution
//		WalkEvalHeadingStartEndVonMises(const double sigma_rad = 0.04) :
//			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
//			;
//		}

//		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

//			PERF_REGION(4, "WalkEvalHeadingStartEnd");

//			Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");

//			const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
//			const float diff = head.getDiffHalfRAD(walk.requested.heading);
//			//const float diff = Heading::getSignedDiff(params.heading, head);
//			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
//			return dist.getProbability(diff);

//		}

//	};

	/**
	 * evaluate the difference between head(start,end) and the requested heading
	 */
	template <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {

		const double sigma_rad;
		Distribution::Normal<double> dist;

	public:

		WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
			sigma_rad(sigma_rad), dist(0, sigma_rad) {
			;
		}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			PERF_REGION(4, "WalkEvalHeadingStartEnd");

			Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");

			const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
			const float diff = head.getDiffHalfRAD(walk.requested.heading);
			//const float diff = Heading::getSignedDiff(params.heading, head);
			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
			return dist.getProbability(diff);

		}

	};


	/**
	 * evaluate the difference between distance(start, end) and the requested distance
	 */
	template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {

		const double sigma;

		const Distribution::Normal<double> dist;

	public:

		WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			PERF_REGION(5, "WalkEvalDistance");

			const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
			const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
			const float diff = walkedDistance_m - requestedDistance_m;
			const double res = dist.getProbability(diff);
			return res;
			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);

		}

	};


	/**
	 * higher probability for points that approach the target location
	 */
	template <typename Tria> class WalkEvalApproachesTarget : public NavMeshWalkEval<Tria> {

		const double p;

	public:

		WalkEvalApproachesTarget(const double pApproaching = 0.65) : p(pApproaching) {
			;
		}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			// sanity check
			StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();

			const NavMeshLocation<Tria> locStart = walk.requested.start;
			const NavMeshLocation<Tria> locEnd = walk.end;

			const float distFromNew = locEnd.tria-> template getDistanceToDestination<Tria>(locEnd.pos);
			const float distFromOld = locStart.tria-> template getDistanceToDestination<Tria>(locStart.pos);

			const double pRemain = 1.0 / (2+distFromNew);

			return ((distFromNew <= distFromOld) ? (p) : (1.0 - p)) + std::pow(pRemain, 0.2);

		}

	};

}

#endif // NAVMESHWALKEVAL_H