1 files changed, 446 insertions, 0 deletions
diff --git a/Runtime/NavMesh/NavMesh.cpp b/Runtime/NavMesh/NavMesh.cpp
new file mode 100644
index 0000000..c9af244
--- /dev/null
+++ b/Runtime/NavMesh/NavMesh.cpp
@@ -0,0 +1,446 @@
+#include "UnityPrefix.h"
+#include "NavMesh.h"
+#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h"
+#include "DetourNavMeshQuery.h"
+#include "NavMeshManager.h"
+#include "NavMeshPath.h"
+#include "NavMeshLayers.h"
+#include "DetourAlloc.h"
+#include "DetourSwapEndian.h"
+#include "HeightMeshQuery.h"
+
+/*
+ TODO:
+
+ 1) Tile boundary coordinates depend on the global bounding box (worldMin + n*size) - make it independent of the global extents (eg. use origo).
+
+ 2) make remainingDistance return distance in all the time (not infinity).
+
+ 3) NavMesh baking bounding box and seed points to cull poly-count.
+
+ 4) Support non-cylinder avoidance obstacles (eg. box).
+
+ 5) Support cylinder shaped obstacle carving.
+
+ 6) Line-to-line offmeshlink type (in addition to the current point-to-point)
+
+*/
+
+NavMesh::NavMesh (MemLabelId& label, ObjectCreationMode mode)
+: Super (label, mode)
+, m_NavMesh (NULL)
+, m_NavMeshQuery (NULL)
+, m_HeightMeshQuery (NULL)
+{
+}
+
+NavMesh::~NavMesh ()
+{
+	Cleanup ();
+}
+
+int NavMesh::CalculatePolygonPath (NavMeshPath* path, const Vector3f& sourcePosition, const Vector3f& targetPosition, const dtQueryFilter& filter)
+{
+	if (m_NavMeshQuery==NULL)
+		return 0;
+
+	float targetMappedPos[3];
+	float sourceMappedPos[3];
+	dtNavMeshQuery* query = m_NavMeshQuery;
+	const float* ext = m_QueryExtents.GetPtr ();
+
+	dtPolyRef targetPolyRef;
+	query->findNearestPoly (targetPosition.GetPtr (), ext, &filter, &targetPolyRef, targetMappedPos);
+	if (targetPolyRef == 0)
+		return 0;
+
+	dtPolyRef sourcePolyRef;
+	query->findNearestPoly (sourcePosition.GetPtr (), ext, &filter, &sourcePolyRef, sourceMappedPos);
+	if (sourcePolyRef == 0)
+		return 0;
+
+	int polygonCount = 0;
+
+	// TODO: Cache an up-to-date filter in navmesh (or manager) to avoid this copy
+	dtQueryFilter filter2 = filter;
+	const NavMeshLayers& layers = GetNavMeshLayers ();
+	for (int i = 0; i < NavMeshLayers::kLayerCount; ++i)
+		filter2.setAreaCost (i, layers.GetLayerCost (i));
+
+	dtStatus status = query->initSlicedFindPath (sourcePolyRef, targetPolyRef, sourceMappedPos, targetMappedPos, &filter2);
+	if (!dtStatusFailed (status))
+		status = query->updateSlicedFindPath (65535, NULL);
+	if (!dtStatusFailed (status))
+		status = query->finalizeSlicedFindPath (path->GetPolygonPath (), &polygonCount, NavMeshPath::kMaxPathPolygons);
+
+	path->SetTimeStamp (m_NavMesh->getTimeStamp ());
+	path->SetPolygonCount (polygonCount);
+	path->SetSourcePosition (Vector3f (sourceMappedPos));
+	path->SetTargetPosition (Vector3f (targetMappedPos));
+	if (dtStatusFailed (status) || polygonCount == 0)
+	{
+		path->SetStatus (kPathInvalid);
+		return 0;
+	}
+
+	if (dtStatusDetail (status, DT_PARTIAL_RESULT))
+	{
+		// when path is partial we project the target position
+		// to the last polygon in the path.
+
+		const dtPolyRef* polygonPath = path->GetPolygonPath ();
+		const dtPolyRef lastPolyRef = polygonPath[polygonCount-1];
+		Vector3f partialTargetPos;
+		dtStatus status = query->closestPointOnPoly (lastPolyRef, targetMappedPos, partialTargetPos.GetPtr ());
+		if (dtStatusFailed (status))
+		{
+			path->SetStatus (kPathInvalid);
+			return 0;
+		}
+
+		path->SetStatus (kPathPartial);
+		path->SetTargetPosition (partialTargetPos);
+	}
+	else
+	{
+		path->SetStatus (kPathComplete);
+	}
+
+	return polygonCount;
+}
+
+int NavMesh::CalculatePathCorners (Vector3f* corners, int maxCorners, const NavMeshPath& path)
+{
+	if (m_NavMeshQuery==NULL)
+		return 0;
+
+	const dtNavMeshQuery* query = m_NavMeshQuery;
+
+	int cornerCount = 0;
+	dtStatus result;
+	Vector3f sourcePos = path.GetSourcePosition ();
+	Vector3f targetPos = path.GetTargetPosition ();
+
+	result = query->findStraightPath (sourcePos.GetPtr (), targetPos.GetPtr (),
+									 path.GetPolygonPath (), path.GetPolygonCount (),
+									 corners[0].GetPtr (), NULL, NULL, &cornerCount, maxCorners);
+	if (result != DT_SUCCESS)
+		return 0;
+	return cornerCount;
+}
+
+void NavMesh::Triangulate (NavMesh::Triangulation& triangulation) const
+{
+	// Mapping between old and new vertex indices.
+	typedef std::map<UInt16, SInt32> VertexMap;
+
+	dynamic_array<SInt32>& layers = triangulation.layers;
+	dynamic_array<SInt32>& indices = triangulation.indices;
+	dynamic_array<Vector3f>& vertices = triangulation.vertices;
+
+	indices.clear ();
+	vertices.clear ();
+
+	const size_t tileCount = m_NavMesh->tileCount ();
+	for (size_t it = 0; it < tileCount; ++it)
+	{
+		const dtMeshTile* tile = m_NavMesh->getTile (it);
+		if (tile == NULL || tile->header == NULL)
+			continue;
+
+		for (size_t ip = 0; ip < tile->header->polyCount; ++ip)
+		{
+			const dtPoly& p = tile->polys[ip];
+			const size_t polyVertCount = p.vertCount;
+
+			// Ignore irregular polygons.
+			if (polyVertCount < 3)
+				continue;
+
+			VertexMap vertexMap;
+
+			// Find or update new vertex index.
+			for (size_t iv = 0; iv < polyVertCount; ++iv)
+			{
+				UInt16 vi = p.verts[iv];
+				VertexMap::iterator found = vertexMap.find (vi);
+				if (found != vertexMap.end ())
+					continue;
+
+				// Lookup the height for vertex
+				dtPolyRef ref = m_NavMesh->getPolyRefBase (tile)|(dtPolyRef)ip;
+				Vector3f vertex = Vector3f (&tile->verts[3*vi]);
+				float ypos;
+				m_NavMeshQuery->getPolyHeight (ref, vertex.GetPtr (), &ypos);
+				vertex.y = ypos;
+
+				vertexMap[vi] = vertices.size ();
+				vertices.push_back (vertex);
+			}
+
+			// Add triangles for this polygon.
+			const SInt32 v0 = vertexMap[p.verts[0]];
+			SInt32 v1 = vertexMap[p.verts[1]];
+			for (size_t iv = 2; iv < polyVertCount; ++iv)
+			{
+				const SInt32 v2 = vertexMap[p.verts[iv]];
+				indices.push_back (v0);
+				indices.push_back (v1);
+				indices.push_back (v2);
+
+				v1 = v2;
+			}
+
+			// Add the navmesh layer for each triangle
+			for (size_t iv = 2; iv < polyVertCount; ++iv)
+			{
+				layers.push_back (p.getArea ());
+			}
+		}
+	}
+}
+
+bool NavMesh::Raycast (NavMeshHit* hit, const Vector3f& sourcePosition, const Vector3f& targetPosition, const dtQueryFilter& filter)
+{
+	dtPolyRef mappedPolyRef;
+	Vector3f mappedPosition;
+	if (!MapPosition (&mappedPolyRef, &mappedPosition, sourcePosition, m_QueryExtents, filter))
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+
+	float st;
+	float hitNormal[3];
+	unsigned int hitPolyFlags;
+	float height;
+	dtStatus result = m_NavMeshQuery->simpleRaycast (mappedPolyRef, mappedPosition.GetPtr (), targetPosition.GetPtr (), &filter, &st, hitNormal, &hitPolyFlags, NULL, &height);
+	if (dtStatusFailed (result))
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+
+	if (st<1.0f)
+	{
+		hit->mask = hitPolyFlags;
+		hit->hit = true;
+		hit->position = Lerp (mappedPosition, targetPosition, st);
+	}
+	else
+	{
+		hit->mask = 0;
+		hit->hit = false;
+		hit->position = targetPosition;
+	}
+
+	hit->position.y = height;
+	hit->distance = Magnitude (hit->position - sourcePosition);
+	hit->normal = Vector3f (hitNormal);
+
+	return hit->hit;
+}
+
+bool NavMesh::SamplePosition (NavMeshHit* hit, const Vector3f& sourcePosition, const dtQueryFilter& filter, float maxDistance)
+{
+	dtPolyRef mappedPolyRef;
+	Vector3f mappedPosition;
+	const Vector3f extents = Vector3f (maxDistance, maxDistance, maxDistance);
+	if (!MapPosition (&mappedPolyRef, &mappedPosition, sourcePosition, extents, filter))
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+
+	const float distance = Magnitude (mappedPosition - sourcePosition);
+	if (distance > maxDistance)
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+
+	hit->mask = m_NavMeshQuery->getPolygonFlags (mappedPolyRef);
+	hit->hit = true;
+	hit->position = mappedPosition;
+	hit->distance = distance;
+	hit->normal = Vector3f::zero;
+	return true;
+}
+
+bool NavMesh::DistanceToEdge (NavMeshHit* hit, const Vector3f& sourcePosition, const dtQueryFilter& filter)
+{
+	dtPolyRef mappedPolyRef;
+	Vector3f mappedPosition;
+	if (!MapPosition (&mappedPolyRef, &mappedPosition, sourcePosition, m_QueryExtents, filter))
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+
+	const dtStatus status = m_NavMeshQuery->findEdge (mappedPolyRef, mappedPosition.GetPtr (), &filter, &hit->mask, hit->normal.GetPtr (), hit->position.GetPtr ());
+	if (dtStatusFailed (status))
+	{
+		InvalidateNavMeshHit (hit);
+		return false;
+	}
+	hit->distance = Magnitude (hit->position - sourcePosition);
+	hit->hit = true;
+
+	return true;
+}
+
+bool NavMesh::MapPosition (dtPolyRef* mappedPolyRef, Vector3f* mappedPosition, const Vector3f& position, const Vector3f& extents, const dtQueryFilter& filter) const
+{
+	if (!m_NavMeshQuery)
+		return false;
+
+	m_NavMeshQuery->findNearestPoly (position.GetPtr (), extents.GetPtr (), &filter, mappedPolyRef, mappedPosition->GetPtr ());
+	const dtPolyRef ref = *mappedPolyRef;
+	return ref != 0;
+}
+
+void NavMesh::Create ()
+{
+	Cleanup ();
+
+	if (m_MeshData.empty ())
+		return;
+
+	Assert (m_NavMesh == NULL);
+	m_NavMesh = dtAllocNavMesh ();
+	if (!m_NavMesh)
+		return CleanupWithError ();
+
+	dtStatus status;
+
+	// try loading tile data
+	status = m_NavMesh->initTiles (GetMeshData (), GetMeshDataSize ());
+	if (dtStatusFailed (status))
+		return CleanupWithError ();
+	if (m_NavMesh->tileCount () == 0)
+		return Cleanup ();
+
+	////@TODO: WTF IS THIS MAGIC NUMBER???
+	Assert (m_NavMeshQuery == NULL);
+	m_NavMeshQuery = dtAllocNavMeshQuery (m_NavMesh, 2048);
+	if (m_NavMeshQuery == NULL)
+		return CleanupWithError ();
+
+	// @TODO: remove tile header redundancy and enforce identical tile parameters.
+	const dtMeshTile* tile = m_NavMesh->getTile (0);
+	if (tile && tile->header)
+	{
+		const float height = tile->header->walkableHeight;
+		const float width = tile->header->walkableRadius;
+		m_QueryExtents = Vector3f (width, height, width);
+
+		// Set up HeightMeshQuery object if the baked data needs it
+		const bool useHeightMeshQuery = (tile->header->flags & DT_MESH_HEADER_USE_HEIGHT_MESH) || !m_Heightmaps.empty ();
+		if (useHeightMeshQuery)
+		{
+			if (!m_HeightMeshQuery)
+			{
+				m_HeightMeshQuery = UNITY_NEW (HeightMeshQuery, kMemNavigation) ();
+				m_HeightMeshQuery->Init (&m_Heightmaps, 1.05f*tile->header->walkableClimb);
+			}
+			m_NavMeshQuery->setHeightQuery (m_HeightMeshQuery);
+		}
+		else
+		{
+			UNITY_DELETE (m_HeightMeshQuery, kMemNavigation);
+			m_NavMeshQuery->setHeightQuery (NULL);
+		}
+	}
+	else
+	{
+		m_QueryExtents = Vector3f (1,1,1);
+	}
+}
+
+void NavMesh::CleanupWithError ()
+{
+	ErrorString ("Creating NavMesh failed");
+	Cleanup ();
+}
+
+void NavMesh::Cleanup ()
+{
+	GetNavMeshManager ().CleanupMeshDependencies (m_NavMesh);
+	if (m_NavMesh)
+	{
+		dtFreeNavMesh (m_NavMesh);
+		m_NavMesh = NULL;
+	}
+	if (m_NavMeshQuery)
+	{
+		dtFreeNavMeshQuery (m_NavMeshQuery);
+		m_NavMeshQuery = NULL;
+	}
+	if (m_HeightMeshQuery)
+	{
+		UNITY_DELETE (m_HeightMeshQuery, kMemNavigation);
+	}
+}
+
+
+void NavMesh::AwakeFromLoad (AwakeFromLoadMode mode)
+{
+	Super::AwakeFromLoad (mode);
+	Create ();
+}
+
+void NavMesh::SetData (const UInt8* data, unsigned size)
+{
+	m_MeshData.assign (data, data + size);
+	Create ();
+}
+
+
+template<class TransferFunction> inline
+void TransferMeshDataByteSwap (TransferFunction& transfer, dynamic_array<UInt8>& data)
+{
+	if (transfer.IsWriting ())
+	{
+		dynamic_array<UInt8> copy = data;
+		if (!copy.empty ())
+		{
+			ErrorIf (!dtNavMeshSetSwapEndian (&copy[0], copy.size ()));
+		}
+		transfer.Transfer (copy, "m_MeshData");
+		return;
+	}
+
+	transfer.Transfer (data, "m_MeshData");
+
+	if (transfer.IsReading () && !data.empty ())
+	{
+		ErrorIf (!dtNavMeshSetSwapEndian (&data[0], data.size ()));
+	}
+}
+
+template<class TransferFunction>
+void NavMesh::Transfer (TransferFunction& transfer)
+{
+	Super::Transfer (transfer);
+
+	if (!transfer.ConvertEndianess ())
+		transfer.Transfer (m_MeshData, "m_MeshData");
+	else
+		TransferMeshDataByteSwap (transfer, m_MeshData);
+
+	TRANSFER (m_Heightmaps);
+}
+
+IMPLEMENT_OBJECT_SERIALIZE (NavMesh)
+IMPLEMENT_CLASS (NavMesh)
+
+
+void InvalidateNavMeshHit (NavMeshHit* hit)
+{
+	const float maxDistance = std::numeric_limits<float>::infinity ();
+	hit->mask = 0;
+	hit->hit = false;
+	hit->position = Vector3f::infinityVec;
+	hit->distance = maxDistance;
+	hit->normal = Vector3f::zero;
+}