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;
		float 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;}


		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);
			return res;

		}



		/** 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.0f / (dot00 * dot11 - dot01 * 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