Indoor/navMesh/NavMeshTriangle.h

#ifndef NAVMESHTRIANGLE_H
#define NAVMESHTRIANGLE_H

#include "../geo/Point3.h"
#include "../geo/Point2.h"

namespace NM {

	/**
	 * represents one triangle within the NavMesh
	 * each Triangle has up to 3 neighbors (one per edge)
	 *
	 * for performance enhancements,
	 * some memeber attributes are pre-calculated once
	 */
	class NavMeshTriangle {

	private:

		template<typename> friend class NavMesh;

		const Point3 p1;
		const Point3 p2;
		const Point3 p3;
		const uint8_t type;

		NavMeshTriangle* _neighbors[3];
		int _numNeighbors;

	private:	// precalculated stuff

		Point2 v0;
		Point2 v1;
		float dot00;
		float dot01;
		float dot11;
		double invDenom;
		float area;

		float minZ;
		float maxZ;

		const Point3 center;
		const Point3 v12;
		const Point3 v13;

	public:

		/** ctor */
		NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
			p1(p1), p2(p2), p3(p3), type(type),
			_neighbors(), _numNeighbors(0),
			center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {

			precompute();

		}

		/** get the triangle's type */
		uint8_t getType() const {return type;}

		Point3 getP1() const {return p1;}

		Point3 getP2() const {return p2;}

		Point3 getP3() const {return p3;}


		/** get the distance between the given point and the triangle using approximate tests */
		float getDistanceApx(const Point3 pt) const {

//			const float d1 = pt.getDistance(p1);
//			const float d2 = pt.getDistance(p2);
//			const float d3 = pt.getDistance(p3);
//			const float d4 = pt.getDistance(center);
//			const float d5 = pt.getDistance((p1-p2)/2);
//			const float d6 = pt.getDistance((p2-p3)/2);
//			const float d7 = pt.getDistance((p3-p1)/2);
//			return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7))))));

//			const float d1 = pt.getDistance(p1);
//			const float d2 = pt.getDistance(p2);
//			const float d3 = pt.getDistance(p3);
//			const float d4 = pt.getDistance(center);
//			return std::min(d1, std::min(d2, std::min(d3,d4)));

			float bestD = 99999;
			Point3 bestP;
			Point3 dir12 = p2-p1;
			Point3 dir13 = p3-p1;
			Point3 dir23 = p3-p2;
			for (float f = 0; f < 1; f += 0.05f) {
				const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt);
				const Point3 pos2 = p1 + dir13 * f;	const float dist2 = pos2.getDistance(pt);
				const Point3 pos3 = p2 + dir23 * f;	const float dist3 = pos3.getDistance(pt);
				if (dist1 < bestD) {bestP = pos1; bestD = dist1;}
				if (dist2 < bestD) {bestP = pos2; bestD = dist2;}
				if (dist3 < bestD) {bestP = pos3; bestD = dist3;}
			}

			return bestD;


		}


		bool operator == (const NavMeshTriangle& o) const {
			return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
		}

		/** is the triangle plain? (same Z for all points) */
		bool isPlain() const {
			const float d1 = std::abs(p1.z - p2.z);
			const float d2 = std::abs(p2.z - p3.z);
			return (d1 < 0.1) && (d2 < 0.1);
		}

		const NavMeshTriangle* const* begin() const {return &_neighbors[0];}

		const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}

		Point3 getPoint(const float u, const float v) const {
			return p1 + (v12*u) + (v13*v);
		}

		/** does the triangle contain the given 3D point? */
		bool contains(const Point3 p) const {
			return	(minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
		}

		/** does the triangle contain the given 2D point? */
		bool contains(const Point2 p) const {

			const Point2 v2 = p - p1.xy();

			// Compute dot products
			float dot02 = dot(v0, v2);
			float dot12 = dot(v1, v2);

			// Compute barycentric coordinates
			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

			// Check if point is in triangle
			return (u >= 0) && (v >= 0) && (u + v <= 1);

		}

		/** estimate the correct z-value for the given 2D point */
		Point3 toPoint3(const Point2 p) const {

			const Point2 v2 = p - p1.xy();

			// Compute dot products
			float dot02 = dot(v0, v2);
			float dot12 = dot(v1, v2);

			// Compute barycentric coordinates
			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

			const Point3 res = getPoint(v,u);
			Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
			Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");

			//return res;
			return Point3(p.x, p.y, res.z);	// only use the new z, keep input as-is

		}



		/** get the triangle's size */
		float getArea() const {
			return area;
		}

		/** get the triangle's center-point */
		Point3 getCenter() const {
			return center;
		}



	private:

		/** perform some pre-calculations to speed things up */
		void precompute() {

	#warning "TODO, z buffer"
			minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15;	// TODO the builder does not align on the same height as we did
			maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;

			// Compute vectors
			v0 = p3.xy() - p1.xy();
			v1 = p2.xy() - p1.xy();

			// Compute dot products
			dot00 = dot(v0, v0);
			dot01 = dot(v0, v1);
			dot11 = dot(v1, v1);

			// Compute barycentric coordinates
			invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);




			const float a = (p2-p1).length();
			const float b = (p3-p1).length();
			const float c = (p2-p3).length();
			const float s = 0.5f * (a+b+c);
			area = std::sqrt( s * (s-a) * (s-b) * (s-c) );

		}

	protected:

		void addNeighbor(NavMeshTriangle* o) {
			Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
			_neighbors[_numNeighbors] = o;
			++_numNeighbors;
		}


	};

}

#endif // NAVMESHTRIANGLE_H