diff options
Diffstat (limited to 'Runtime/NavMesh')
36 files changed, 7113 insertions, 0 deletions
diff --git a/Runtime/NavMesh/DynamicMesh.cpp b/Runtime/NavMesh/DynamicMesh.cpp new file mode 100644 index 0000000..e4d54a7 --- /dev/null +++ b/Runtime/NavMesh/DynamicMesh.cpp @@ -0,0 +1,693 @@ +#include "UnityPrefix.h" +#include "./DynamicMesh.h" +#include <math.h> +#include <float.h> +#include "Runtime/Math/FloatConversion.h" +#include "DetourCommon.h" + +const float MAGIC_QUANTIZE = 1e-2f; +const bool MERGE_POLYGONS = true; + +// Compiling to conditional move this is typically faster than modulus operator % in the general case +static inline size_t NextIndex (size_t index, size_t modulus) +{ + DebugAssert (index < modulus); + const size_t next = index + 1; + return (next == modulus) ? 0 : next; +} + +// Compiling to conditional move this is typically faster than modulus operator % in the general case +static inline size_t PrevIndex (size_t index, size_t modulus) +{ + DebugAssert (index < modulus); + return (index == 0) ? modulus-1 : index - 1; +} + +// TODO : round only fraction part of the floats +// in order to avoid integer overflow for large values of 'v' +static inline Vector3f QuantizeVertex (const Vector3f& v) +{ + if (MAGIC_QUANTIZE <= 0) + { + return v; + } + else + { + Vector3f qv = 1.0f / MAGIC_QUANTIZE * v; + qv = Vector3f (RoundfToInt (qv.x), RoundfToInt (qv.y), RoundfToInt (qv.z)) * MAGIC_QUANTIZE; + return qv; + } +} + +static inline bool IsQuantized (const Vector3f& v) +{ + Vector3f qv = QuantizeVertex (v); + return qv == v; +} + +static inline bool IsStrictlyConvex (const dynamic_array< Vector3f >& vertices) +{ + const size_t vertexCount = vertices.size (); + for (size_t i = 0; i < vertexCount; ++i) + { + const float* v0 = vertices[PrevIndex (i, vertexCount)].GetPtr (); + const float* v1 = vertices[i].GetPtr (); + const float* v2 = vertices[NextIndex (i, vertexCount)].GetPtr (); + const float triArea = dtTriArea2D (v0, v1, v2); + if (triArea <= 0) return false; + } + return true; +} + +static inline bool PolygonDegenerate (size_t vertexCount, const UInt16* indices, const Vector3f* vertices) +{ + for (size_t i = 0; i < vertexCount; ++i) + { + DebugAssert (IsQuantized (vertices[indices[i]])); + } + if (vertexCount < 3) + { + return true; + } + float area = 0.0f; + float maxSideSq = 0.0f; + for (size_t i = 2; i < vertexCount; ++i) + { + const float* v0 = vertices[indices[0]].GetPtr (); + const float* v1 = vertices[indices[i-1]].GetPtr (); + const float* v2 = vertices[indices[i]].GetPtr (); + const float triArea = dtTriArea2D (v0, v1, v2); + + area += triArea; + maxSideSq = std::max (dtVdistSqr (v0, v1), maxSideSq); + maxSideSq = std::max (dtVdistSqr (v0, v2), maxSideSq); + } + if (area <= 0) + { + return true; + } + const float safety = 1e-2f * MAGIC_QUANTIZE; + return area * area <= safety * safety * maxSideSq; +} + +static inline float PolygonDegenerate (const dynamic_array< Vector3f >& vertices) +{ + if (vertices.size () < 3) + { + return true; + } + float area = 0.0f; + float maxSideSq = 0.0f; + for (size_t i = 2; i < vertices.size (); ++i) + { + const float* v0 = vertices[0].GetPtr (); + const float* v1 = vertices[i-1].GetPtr (); + const float* v2 = vertices[i].GetPtr (); + const float triArea = dtTriArea2D (v0, v1, v2); + + area += triArea; + maxSideSq = std::max (dtVdistSqr (v0, v1), maxSideSq); + maxSideSq = std::max (dtVdistSqr (v0, v2), maxSideSq); + } + if (area <= 0) + { + return true; + } + const float safety = 1e-2f * MAGIC_QUANTIZE; + return area * area <= safety * safety * maxSideSq; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +size_t DynamicMesh::AddVertexChecked (const Vector3f& v) +{ + const Vector3f qv = QuantizeVertex (v); + size_t vertexCount = m_Vertices.size (); + for (size_t iv = 0; iv < vertexCount; ++iv) + { + const Vector3f& ov = m_Vertices[iv]; + if (qv == ov) + return iv; + } + + m_Vertices.push_back (qv); + return vertexCount; +} + +DynamicMesh::Poly DynamicMesh::CreatePolygon (const Polygon& vertices, const UInt32 status) +{ + size_t vertexCount = vertices.size (); + DebugAssert (vertexCount <= NUM_VERTS); + DebugAssert (vertexCount > 2); + + Poly newPoly = {{0}, {0}, 0, 0}; + newPoly.m_VertexCount = vertexCount; + newPoly.m_Status = status; + for (size_t i = 0; i < vertexCount; ++i) + { + size_t vi = AddVertexChecked (vertices[i]); + DebugAssert (vi < 0xffff); //< vertex overflow + newPoly.m_VertexIDs[i] = (UInt16)vi; + } + return newPoly; +} + +void DynamicMesh::RemovePolygon (size_t i) +{ + DebugAssert (i < m_Polygons.size ()); + DebugAssert (m_Data.size () == m_Polygons.size ()); + + m_Polygons.erase (m_Polygons.begin () + i); + m_Data.erase (m_Data.begin () + i); +} + +// Clip the convex polygon 'poly' by the half-space defined by 'plane' +void DynamicMesh::SplitPoly (Polygon& inside, const Polygon& poly, const Plane& plane) const +{ + inside.resize_uninitialized (0); + const size_t vertexCount = poly.size (); + + dynamic_array< float > dist (vertexCount, kMemTempAlloc); + for (size_t iv = 0; iv < vertexCount; ++iv) + { + const Vector3f& v = poly[iv]; + dist[iv] = plane.GetDistanceToPoint (v); + } + + Vector3f prevVert = poly[vertexCount-1]; + float prevDist = dist[vertexCount-1]; + + for (size_t iv = 0; iv < vertexCount; ++iv) + { + const Vector3f& currVert = poly[iv]; + const float currDist = dist[iv]; + + if (currDist < 0 && prevDist > 0) + { + const float absDist = -currDist; + const float w = absDist / (absDist + prevDist); + const Vector3f newVert = Lerp (currVert, prevVert, w); + inside.push_back (newVert); + // inside.push_back (QuantizeVertex (newVert)); + } + else if (currDist > 0 && prevDist < 0) + { + const float absDist = -prevDist; + const float w = absDist / (absDist + currDist); + const Vector3f newVert = Lerp (prevVert, currVert, w); + inside.push_back (newVert); + // inside.push_back (QuantizeVertex (newVert)); + } + + if (currDist <= 0) + { + inside.push_back (currVert); + } + + prevVert = currVert; + prevDist = currDist; + } +} + +// Return the intersection of 'poly' and 'carveHull' +// Assuming convex shapes. +void DynamicMesh::Intersection (Polygon& inside, const Polygon& poly, const Hull& carveHull) const +{ + DebugAssert (inside.empty ()); + const size_t planeCount = carveHull.size (); + inside.reserve (planeCount + NUM_VERTS); + inside = poly; + + Polygon insidePoly; + insidePoly.reserve (planeCount + NUM_VERTS); + + for (size_t ic = 0; ic < planeCount; ++ic) + { + const Plane& plane = carveHull[ic]; + SplitPoly (insidePoly, inside, plane); + if (insidePoly.empty ()) + { + inside.resize_uninitialized (0); + break; + } + inside = insidePoly; + } +} + +void DynamicMesh::FromPoly (Polygon& result, const Poly& poly) const +{ + DebugAssert (poly.m_VertexCount > 2); + DebugAssert (poly.m_VertexCount <= NUM_VERTS); + + const UInt32 vertexCount = poly.m_VertexCount; + result.resize_uninitialized (vertexCount); + + for (size_t i = 0; i < vertexCount; ++i) + { + result[i] = Vector3f (GetVertex (poly.m_VertexIDs[i])); + } +} + +void DynamicMesh::BuildEdgeConnections (EdgeList& edges) const +{ + DebugAssert (edges.empty ()); + const size_t polyCount = m_Polygons.size (); + for (size_t ip = 0; ip < polyCount; ++ip) + { + const Poly& poly = m_Polygons[ip]; + if (PolygonDegenerate (poly.m_VertexCount, poly.m_VertexIDs, &m_Vertices[0])) + continue; + + size_t vertexCount = poly.m_VertexCount; + + for (size_t ivp = vertexCount-1, iv = 0; iv < vertexCount; ivp = iv++) + { + UInt16 vp = poly.m_VertexIDs[ivp]; + UInt16 v = poly.m_VertexIDs[iv]; + + DebugAssert (v != vp); + + // Find edge by ordered vertex indices + UInt16 vmin = (v<vp) ? v : vp; + UInt16 vmax = (v>vp) ? v : vp; + + size_t ie = 0; + size_t edgeCount = edges.size (); + for (; ie < edgeCount; ++ie) + { + Edge& edge = edges[ie]; + if (edge.v1 != vmin || edges[ie].v2 != vmax) + continue; + + // If already connected skip it. + // A polygon edge cannot connect more than two polygons. + // Ideally this should not happen. + if (edge.c2 != 0xffff) + break; + + // Found an existing unconnected edge + edge.p2 = ip; + edge.c2 = ivp; + break; + } + // Edge not found - insert + if (ie == edgeCount) + { + Edge edge = + { vmin, vmax, ip, 0xffff, ivp, 0xffff }; + edges.push_back (edge); + } + } + } +} + +// Locate furthest vertex in positive half-plane or -1 in none found. +int DynamicMesh::FindFurthest (const Vector3f& v1, const Vector3f& v2, const VertexContainer& vertices) const +{ + int bestIndex = -1; + float bestDist = 0; + + for (size_t iv = 0; iv < vertices.size (); ++iv) + { + const float* v = vertices[iv].GetPtr (); + float dist = dtTriArea2D(v1.GetPtr (), v2.GetPtr (), v); + if (dist > bestDist) + { + bestDist = dist; + bestIndex = iv; + } + } + return bestIndex; +} + +void DynamicMesh::Subtract (PolygonContainer& result, const Polygon& outer, const Polygon& inner) const +{ + const size_t innerVertexCount = inner.size (); + const size_t outerVertexCount = outer.size (); + result.clear (); + Polygon tri (3, kMemTempAlloc); + + if (innerVertexCount == 1) + { + DebugAssert (outerVertexCount > 0); + for (size_t ov = 0; ov < outerVertexCount; ++ov) + { + const size_t ovn = NextIndex (ov, outerVertexCount); + tri[0] = outer[ov]; + tri[1] = outer[ovn]; + tri[2] = inner[0]; + if (PolygonDegenerate (tri)) + { + continue; + } + tri[2] = QuantizeVertex (inner[0]); + if (PolygonDegenerate (tri)) + { + continue; + } + result.push_back (tri); + } + return; + } + + dynamic_array< int > ol (innerVertexCount, -1, kMemTempAlloc); + dynamic_array< int > oh (innerVertexCount, -1, kMemTempAlloc); + dynamic_array< bool > used (outerVertexCount, false, kMemTempAlloc); + + for (size_t ivp = innerVertexCount-1, iv = 0; iv < innerVertexCount; ivp = iv++) + { + int bestOuter = FindFurthest (inner[iv], inner[ivp], outer); + + if (bestOuter == -1) + { + continue; + } + ol[iv] = bestOuter; + oh[ivp] = bestOuter; + + tri[0] = inner[iv]; + tri[1] = inner[ivp]; + tri[2] = outer[bestOuter]; + + if (PolygonDegenerate (tri)) + { + continue; + } + tri[0] = QuantizeVertex (inner[iv]); + tri[1] = QuantizeVertex (inner[ivp]); + if (PolygonDegenerate (tri)) + { + continue; + } + result.push_back (tri); + } + + for (size_t iv = 0; iv < innerVertexCount; ++iv) + { + if (ol[iv] != -1) + { + size_t ov = ol[iv]; + size_t iter = 0; + while (ov != (size_t)oh[iv]) + { + const size_t ovn = NextIndex (ov, outerVertexCount); + if (!used[ov]) + { + tri[0] = outer[ov]; + tri[1] = outer[ovn]; + tri[2] = inner[iv]; + if (PolygonDegenerate (tri)) + { + break; + } + tri[2] = QuantizeVertex (inner[iv]); + if (PolygonDegenerate (tri)) + { + break; + } + result.push_back (tri); + used[ov] = true; + } + ov = ovn; + if (++iter == outerVertexCount) + { + break; + } + } + } + + if (oh[iv] != -1) + { + size_t ov = oh[iv]; + size_t iter = 0; + while (ov != (size_t)ol[iv]) + { + const size_t ovp = PrevIndex (ov, outerVertexCount); + if (!used[ovp]) + { + tri[0] = outer[ovp]; + tri[1] = outer[ov]; + tri[2] = inner[iv]; + if (PolygonDegenerate (tri)) + { + break; + } + tri[2] = QuantizeVertex (inner[iv]); + if (PolygonDegenerate (tri)) + { + break; + } + result.push_back (tri); + used[ovp] = true; + } + ov = ovp; + if (++iter == outerVertexCount) + { + break; + } + } + } + } +} + +bool DynamicMesh::MergePolygons (Polygon& merged, const Polygon& p1, const Polygon& p2) const +{ + if (!MERGE_POLYGONS) + return false; + const size_t count1 = p1.size (); + const size_t count2 = p2.size (); + + if (count1 < 3) return false; + if (count2 < 3) return false; + if ((count1 + count2 - 2) > NUM_VERTS) + return false; + + for (size_t iv = 0; iv < count1; ++iv) + { + const size_t ivn = NextIndex (iv, count1); + const Vector3f& v1 = p1[iv]; + const Vector3f& v2 = p1[ivn]; + for (size_t jv = 0; jv < count2; ++jv) + { + const size_t jvn = NextIndex (jv, count2); + const Vector3f& w1 = p2[jv]; + const Vector3f& w2 = p2[jvn]; + if ((v1 == w2) && (v2 == w1)) + { + // Found shared edge + + // Test convexity + const Vector3f& wn = p2[NextIndex (jvn, count2)]; + const Vector3f& vp = p1[PrevIndex (iv, count1)]; + if (dtTriArea2D (vp.GetPtr (), v1.GetPtr (), wn.GetPtr ()) <= 0) + { + return false; + } + + // Test convexity + const Vector3f& wp = p2[PrevIndex (jv, count2)]; + const Vector3f& vn = p1[NextIndex (ivn, count1)]; + if (dtTriArea2D (v2.GetPtr (), vn.GetPtr (), wp.GetPtr ()) <= 0) + { + return false; + } + + // Merge two polygon parts + for (size_t k = ivn ; k != iv ; k = NextIndex (k, count1)) + { + merged.push_back (p1[k]); + } + for (size_t k = jvn ; k != jv ; k = NextIndex (k, count2)) + { + merged.push_back (p2[k]); + } + DebugAssert (merged.size () == count1 + count2 - 2); + return IsStrictlyConvex (merged); + } + } + } + return false; +} + +void DynamicMesh::MergePolygons () +{ + // Merge list of convex non-overlapping polygons assuming identical data. + for (size_t ip = 0; ip < m_Polygons.size (); ++ip) + { + Polygon poly; + FromPoly (poly, m_Polygons[ip]); + for (size_t jp = m_Polygons.size () - 1; jp > ip; --jp) + { + bool dataConforms = (m_Data[ip] == m_Data[jp]); + if (!dataConforms) + continue; + + Polygon merged; + Polygon poly2; + FromPoly (poly2, m_Polygons[jp]); + if (MergePolygons (merged, poly, poly2)) + { + poly = merged; + m_Polygons.erase (m_Polygons.begin () + jp); + } + if (poly.size () == NUM_VERTS) break; + } + m_Polygons[ip] = CreatePolygon (poly, kGeneratedPolygon); + } +} + +void DynamicMesh::MergePolygons (PolygonContainer& polys) +{ + // Merge list of convex non-overlapping polygons assuming identical data. + for (size_t ip = 0; ip < polys.size (); ++ip) + { + Polygon poly = polys[ip]; + for (size_t jp = polys.size ()-1; jp>ip; --jp) + { + Polygon merged; + if (MergePolygons (merged, poly, polys[jp])) + { + poly = merged; + polys.erase (polys.begin () + jp); + } + } + polys[ip] = poly; + } +} + +void DynamicMesh::ConnectPolygons () +{ + EdgeList edges; + BuildEdgeConnections (edges); + + size_t edgeCount = edges.size (); + for (size_t ie = 0; ie < edgeCount; ++ie) + { + const Edge& edge = edges[ie]; + if (edge.c2 == 0xffff) + continue; + m_Polygons[edge.p1].m_Neighbours[edge.c1] = edge.p2+1; + m_Polygons[edge.p2].m_Neighbours[edge.c2] = edge.p1+1; + } +} + +void DynamicMesh::RemoveDegeneratePolygons () +{ + size_t count = m_Polygons.size (); + for (size_t ip = 0; ip < count; ++ip) + { + if (PolygonDegenerate (m_Polygons[ip].m_VertexCount, m_Polygons[ip].m_VertexIDs, &m_Vertices[0])) + { + RemovePolygon (ip); + --count; + --ip; + } + } +} + +void DynamicMesh::FindNeighbors () +{ + RemoveDegeneratePolygons (); + ConnectPolygons (); +} + +void DynamicMesh::AddPolygon (const Polygon& vertices, const DataType& data) +{ + AddPolygon (vertices, data, kOriginalPolygon); +} + +void DynamicMesh::AddPolygon (const Polygon& vertices, const DataType& data, const UInt32 status) +{ + // Delaying neighbor connections. + DebugAssert (m_Polygons.size () < 0xffff); //< poly overflow + DebugAssert (vertices.size () <= NUM_VERTS); + DebugAssert (m_Data.size () == m_Polygons.size ()); + + if (PolygonDegenerate (vertices)) + { + return; + } + + DebugAssert (IsStrictlyConvex (vertices)); + + Poly newPoly = CreatePolygon (vertices, status); + + // TODO: avoid leaking vertices when not accepting the poly + if (PolygonDegenerate (newPoly.m_VertexCount, newPoly.m_VertexIDs, &m_Vertices[0])) + { + return; + } + m_Polygons.push_back (newPoly); + m_Data.push_back (data); +} + +bool DynamicMesh::ClipPolys (const HullContainer& carveHulls) +{ + size_t hullCount = carveHulls.size (); + PolygonContainer outsidePolygons; + bool clipped = false; + + for (size_t ih = 0; ih < hullCount; ++ih) + { + Hull carveHull = carveHulls[ih]; + + size_t count = m_Polygons.size (); + for (size_t ip = 0; ip < count; ++ip) + { + Polygon currentPoly; + FromPoly (currentPoly, m_Polygons[ip]); + + Polygon inside; + Intersection (inside, currentPoly, carveHull); + if (inside.empty ()) + continue; + + clipped = true; + + Subtract (outsidePolygons, currentPoly, inside); + MergePolygons (outsidePolygons); + + DataType currentData = m_Data[ip]; + RemovePolygon (ip); + --count; + --ip; + + for (size_t io = 0; io < outsidePolygons.size (); ++io) + { + AddPolygon (outsidePolygons[io], currentData, kGeneratedPolygon); + } + } + } + return clipped; +} + +void DynamicMesh::Reserve (const int vertexCount, const int polygonCount) +{ + m_Polygons.reserve (polygonCount); + m_Data.reserve (polygonCount); + m_Vertices.reserve (vertexCount); +} + +void DynamicMesh::AddVertex (const Vector3f& v) +{ + const Vector3f qv = QuantizeVertex (v); + m_Vertices.push_back (qv); +} + +void DynamicMesh::AddPolygon (const UInt16* vertexIDs, const DataType& data, size_t vertexCount) +{ + // TODO : figure out why this needs to be zero'ed + Poly poly = {{0}, {0}, 0, 0}; + + poly.m_Status = kOriginalPolygon; + poly.m_VertexCount = vertexCount; + for (size_t iv = 0; iv < vertexCount; ++iv) + { + poly.m_VertexIDs[iv] = vertexIDs[iv]; + } + m_Polygons.push_back (poly); + m_Data.push_back (data); +} + + diff --git a/Runtime/NavMesh/DynamicMesh.h b/Runtime/NavMesh/DynamicMesh.h new file mode 100644 index 0000000..f815c66 --- /dev/null +++ b/Runtime/NavMesh/DynamicMesh.h @@ -0,0 +1,121 @@ +#ifndef _DYNAMICMESH_H_INCLUDED_ +#define _DYNAMICMESH_H_INCLUDED_ + +#include "Runtime/Math/Vector3.h" +#include "Runtime/Geometry/Plane.h" +#include "Runtime/Utilities/dynamic_array.h" +#include <vector> + +// TODO handle T-junctions (produced by merging / culling degenerate polys). +// TODO cleanup orphan vertices (remap indices) +// TODO optimize using bv-tree to collect source polygons for carving +// TODO re-create bv-tree for faster lookup (possibly by modifying source bv-tree). + +class DynamicMesh +{ + struct Edge + { + UInt16 v1, v2, p1, p2, c1, c2; + }; + typedef dynamic_array< Edge > EdgeList; +public: + + enum + { + NUM_VERTS = 6 + }; + enum + { + kOriginalPolygon = 0, + kGeneratedPolygon = 1 + }; + struct Poly + { + UInt16 m_Neighbours[NUM_VERTS]; + UInt16 m_VertexIDs[NUM_VERTS]; + UInt32 m_VertexCount; + UInt32 m_Status; + }; + + typedef int DataType; + typedef dynamic_array< Plane > Hull; + typedef std::vector< Hull > HullContainer; + + typedef dynamic_array< Vector3f > VertexContainer; + typedef VertexContainer Polygon; + typedef std::vector< Polygon > PolygonContainer; + + inline void Clear (); + inline size_t PolyCount () const; + inline size_t VertCount () const; + inline const float* GetVertex (size_t i) const; + inline const Poly* GetPoly (size_t i) const; + inline const DataType* GetData (size_t i) const; + + void MergePolygons (); + void FindNeighbors (); + void AddPolygon (const Polygon& vertices, const DataType& data); + bool ClipPolys (const HullContainer& carveHulls); + + void Reserve (const int vertexCount, const int polygonCount); + void AddVertex (const Vector3f& v); + void AddPolygon (const UInt16* vertexIDs, const DataType& data, size_t vertexCount); + +private: + size_t AddVertexChecked (const Vector3f& v); + void AddPolygon (const Polygon& vertices, const DataType& data, const UInt32 status); + Poly CreatePolygon (const Polygon& vertices, const UInt32 status); + void RemovePolygon (size_t i); + + void Intersection (Polygon& inside, const Polygon& poly, const Hull& clipHull) const; + void SplitPoly (Polygon& inside, const Polygon& poly, const Plane& plane) const; + void FromPoly (Polygon& result, const Poly& poly) const; + void BuildEdgeConnections (EdgeList& edges) const; + int FindFurthest (const Vector3f& v1, const Vector3f& v2, const VertexContainer& vertices) const; + void Subtract (PolygonContainer& result, const Polygon& outer, const Polygon& inner) const; + void ConnectPolygons (); + void RemoveDegeneratePolygons (); + void MergePolygons (PolygonContainer& polys); + bool MergePolygons (Polygon& merged, const Polygon& p1, const Polygon& p2) const; + + dynamic_array<Poly> m_Polygons; + dynamic_array<Vector3f> m_Vertices; + dynamic_array<DataType> m_Data; +}; + +inline void DynamicMesh::Clear () +{ + m_Polygons.resize_uninitialized (0); + m_Vertices.resize_uninitialized (0); + m_Data.resize_uninitialized (0); +} + +inline size_t DynamicMesh::PolyCount () const +{ + return m_Polygons.size (); +} + +inline size_t DynamicMesh::VertCount () const +{ + return m_Vertices.size (); +} + +inline const float* DynamicMesh::GetVertex (size_t i) const +{ + DebugAssert (i < VertCount ()); + return m_Vertices[i].GetPtr (); +} + +inline const DynamicMesh::Poly* DynamicMesh::GetPoly (size_t i) const +{ + DebugAssert (i < PolyCount ()); + return &m_Polygons[i]; +} + +inline const DynamicMesh::DataType* DynamicMesh::GetData (size_t i) const +{ + DebugAssert (i < PolyCount ()); + return &m_Data[i]; +} + +#endif diff --git a/Runtime/NavMesh/DynamicMeshTests.cpp b/Runtime/NavMesh/DynamicMeshTests.cpp new file mode 100644 index 0000000..8f4d230 --- /dev/null +++ b/Runtime/NavMesh/DynamicMeshTests.cpp @@ -0,0 +1,280 @@ +#include "UnityPrefix.h" +#include "Configuration/UnityConfigure.h" + +#if ENABLE_UNIT_TESTS +#include "External/UnitTest++/src/UnitTest++.h" +#include "DynamicMesh.h" + +SUITE (DynamicMeshTests) +{ + struct DynamicMeshTestFixture + { + DynamicMeshTestFixture () + { + data = 0; + data2 = 2; + + goodPolygon.push_back (Vector3f (0,0,0)); + goodPolygon.push_back (Vector3f (0,0,1)); + goodPolygon.push_back (Vector3f (1,0,1)); + + goodPolygonNeighbor.push_back (Vector3f (0,0,0)); + goodPolygonNeighbor.push_back (Vector3f (1,0,1)); + goodPolygonNeighbor.push_back (Vector3f (1,0,0)); + + upsideDownPolygon.push_back (Vector3f (0,0,0)); + upsideDownPolygon.push_back (Vector3f (1,0,1)); + upsideDownPolygon.push_back (Vector3f (0,0,1)); + + degeneratePolygon.push_back (Vector3f (0,0,0)); + degeneratePolygon.push_back (Vector3f (1,0,0)); + degeneratePolygon.push_back (Vector3f (2,0,0)); + + degeneratePolygon2.push_back (Vector3f (0,0,0)); + degeneratePolygon2.push_back (Vector3f (1,0,0)); + } + + DynamicMesh mesh; + unsigned char data; + unsigned char data2; + + DynamicMesh::Polygon goodPolygon; + DynamicMesh::Polygon goodPolygonNeighbor; + DynamicMesh::Polygon degeneratePolygon; + DynamicMesh::Polygon degeneratePolygon2; + DynamicMesh::Polygon upsideDownPolygon; + }; + + // Create a container for DynamicMesh to use for carving. + // Hull is a simple half-space defined by world-space position and normal. + DynamicMesh::HullContainer HullsFromNormalAndPosition (const Vector3f& normal, const Vector3f& position) + { + Plane plane; + plane.SetNormalAndPosition (normal, position); + + DynamicMesh::Hull hull; + hull.push_back (plane); + + DynamicMesh::HullContainer hulls; + hulls.push_back (hull); + + return hulls; + } + + TEST_FIXTURE (DynamicMeshTestFixture, Construction) + { + CHECK (mesh.PolyCount () == 0); + CHECK (mesh.VertCount () == 0); + } + + TEST_FIXTURE (DynamicMeshTestFixture, AddPolygon) + { + mesh.AddPolygon (goodPolygon, data); + + CHECK (mesh.PolyCount () == 1); + CHECK (mesh.VertCount () == 3); + } + + TEST_FIXTURE (DynamicMeshTestFixture, AddPolygon_IgnoreDegeneratePolygon) + { + mesh.AddPolygon (degeneratePolygon, data); + mesh.AddPolygon (degeneratePolygon2, data); + + CHECK (mesh.PolyCount () == 0); + CHECK (mesh.VertCount () == 0); + } + + TEST_FIXTURE (DynamicMeshTestFixture, AddPolygon_IgnoreUpsideDown) + { + mesh.AddPolygon (upsideDownPolygon, data); + + CHECK (mesh.PolyCount () == 0); + CHECK (mesh.VertCount () == 0); + } + + TEST_FIXTURE (DynamicMeshTestFixture, AddPolygon_SameTwice) + { + mesh.AddPolygon (goodPolygon, data); + mesh.AddPolygon (goodPolygon, data); + + CHECK (mesh.PolyCount () == 2); + CHECK (mesh.VertCount () == 3); + } + + TEST_FIXTURE (DynamicMeshTestFixture, MergePolygonsWithSameData) + { + mesh.AddPolygon (goodPolygon, data); + mesh.AddPolygon (goodPolygonNeighbor, data); + mesh.MergePolygons (); + + CHECK (mesh.PolyCount () == 1); + CHECK (mesh.VertCount () == 4); + } + + TEST_FIXTURE (DynamicMeshTestFixture, DontMergePolygonsWithDifferentData) + { + mesh.AddPolygon (goodPolygon, data); + mesh.AddPolygon (goodPolygonNeighbor, data2); + mesh.MergePolygons (); + + CHECK (mesh.PolyCount () == 2); + CHECK (mesh.VertCount () == 4); + } + + // Verify mesh contains exactly one triangle. Return the area weighted normal. + // ie. vector in direction of the triangle normal - with a magnitude equal to the triangle area. + Vector3f CheckSingleTriangleGetAreaNormal (DynamicMesh& mesh) + { + // Verify a single polygon is left + CHECK (mesh.PolyCount () == 1); + + // .. and it's a triangle + const DynamicMesh::Poly* poly = mesh.GetPoly (0); + CHECK (poly->m_VertexCount == 3); + + const Vector3f v0 = Vector3f (mesh.GetVertex (poly->m_VertexIDs[0])); + const Vector3f v1 = Vector3f (mesh.GetVertex (poly->m_VertexIDs[1])); + const Vector3f v2 = Vector3f (mesh.GetVertex (poly->m_VertexIDs[2])); + const Vector3f triangleAreaNormal = 0.5f*Cross (v1 - v0, v2 - v0); + return triangleAreaNormal; + } + + TEST_FIXTURE (DynamicMeshTestFixture, ClipTriangleWithPlane_Result_ClippedTriangle) + { + // Cut everything z > 0.5f + DynamicMesh::HullContainer carveHulls = HullsFromNormalAndPosition (-Vector3f::zAxis, Vector3f (0.0f, 0.0f, 0.5f)); + + mesh.AddPolygon (goodPolygon, data); + mesh.ClipPolys (carveHulls); + + // Expect a triangle in the horizontal plane with area 1/8 + const Vector3f expectedAreaNormal = Vector3f (0.0f, 0.125f, 0.0f); + const Vector3f triangleAreaNormal = CheckSingleTriangleGetAreaNormal (mesh); + + CHECK (CompareApproximately (expectedAreaNormal, triangleAreaNormal)); + } + + TEST_FIXTURE (DynamicMeshTestFixture, ClipTriangleWithPlane_Result_OriginalTriangle) + { + // Cut everything z > 1.0f + DynamicMesh::HullContainer carveHulls = HullsFromNormalAndPosition (-Vector3f::zAxis, Vector3f (0.0f, 0.0f, 1.0f)); + + mesh.AddPolygon (goodPolygon, data); + + mesh.ClipPolys (carveHulls); + + // Expect a triangle in the horizontal plane with area 1/2 + const Vector3f expectedAreaNormal = Vector3f (0.0f, 0.5f, 0.0f); + const Vector3f triangleAreaNormal = CheckSingleTriangleGetAreaNormal (mesh); + CHECK (CompareApproximately (expectedAreaNormal, triangleAreaNormal)); + } + + TEST_FIXTURE (DynamicMeshTestFixture, ClipTriangleWithPlane_Result_NoTriangle) + { + // Cut everything z > 0 + DynamicMesh::HullContainer carveHulls = HullsFromNormalAndPosition (-Vector3f::zAxis, Vector3f (0.0f, 0.0f, 0.0f)); + + mesh.AddPolygon (goodPolygon, data); + mesh.ClipPolys (carveHulls); + + // Verify that the polygon is removed + CHECK (mesh.PolyCount () == 0); + } + + TEST_FIXTURE (DynamicMeshTestFixture, SplitTriangleIntoTwoPolygons) + { + // Split polygon into two + Vector3f planePos(0.0f, 0.0f, 0.5f); + Plane planeLeft, planeRight; + planeLeft.SetNormalAndPosition (-Vector3f::zAxis, planePos); + planeRight.SetNormalAndPosition (Vector3f::zAxis, planePos); + + DynamicMesh::Hull hull; + hull.push_back (planeLeft); + hull.push_back (planeRight); + + DynamicMesh::HullContainer carveHulls; + carveHulls.push_back (hull); + + mesh.AddPolygon (goodPolygon, data); + mesh.ClipPolys (carveHulls); + + // Verify that the polygon is cut in half + CHECK (mesh.PolyCount () == 2); + } + + static bool HasNeighbor (const DynamicMesh::Poly* poly, int neighborId) + { + for (int i = 0; i < poly->m_VertexCount; i++) + if (poly->m_Neighbours[i] == neighborId) + return true; + return false; + } + + TEST_FIXTURE (DynamicMeshTestFixture, CheckMeshConnectivity) + { + mesh.AddPolygon (goodPolygon, data); + mesh.AddPolygon (goodPolygonNeighbor, data2); + mesh.MergePolygons (); + mesh.FindNeighbors (); + + CHECK (mesh.PolyCount () == 2); + CHECK (mesh.VertCount () == 4); + + // Check that polygon A is connected to polygon B. + const DynamicMesh::Poly* pa = mesh.GetPoly (0); + CHECK (HasNeighbor (pa, 2)); // One based neighbor indices. + + // Check that polygon B is connected to polygon A. + const DynamicMesh::Poly* pb = mesh.GetPoly (1); + CHECK (HasNeighbor (pb, 1)); + } + + + static DynamicMesh::Hull HullFromPolygon (const dynamic_array<Vector3f>& points) + { + DynamicMesh::Hull hull; + for (int i = 0, j = points.size()-1; i < points.size(); j = i++) + { + Vector3f edgeDir = points[i] - points[j]; + Vector3f edgeNormal = Normalize (Vector3f (-edgeDir.z, 0.0f, edgeDir.x)); + Plane plane; + plane.SetNormalAndPosition (edgeNormal, points[j]); + hull.push_back (plane); + } + return hull; + } + + TEST_FIXTURE (DynamicMeshTestFixture, CutTriangleWithRectangle) + { + // Cut triangle with rectangle + // + // o------o + // | x../..x + // | :/ : + // | /: : + // o x.....x + // + dynamic_array<Vector3f> points; + points.push_back (Vector3f (0.25f, 0, 0)); + points.push_back (Vector3f (0.25f, 0, 0.75f)); + points.push_back (Vector3f (1.0f, 0, 0.75f)); + points.push_back (Vector3f (1.0f, 0, 0)); + DynamicMesh::Hull hull = HullFromPolygon (points); + + DynamicMesh::HullContainer carveHulls; + carveHulls.push_back (hull); + + mesh.AddPolygon (goodPolygon, data); + mesh.ClipPolys (carveHulls); + + // Verify that there are more polygons after clipping, because result is non-convex. + CHECK (mesh.PolyCount () > 1); + // Verify that there are 6 vertices. + CHECK (mesh.VertCount() == 6); + } + + +} + +#endif diff --git a/Runtime/NavMesh/HeightMeshQuery.cpp b/Runtime/NavMesh/HeightMeshQuery.cpp new file mode 100644 index 0000000..2e26b6b --- /dev/null +++ b/Runtime/NavMesh/HeightMeshQuery.cpp @@ -0,0 +1,134 @@ +#include "UnityPrefix.h" +#include "HeightMeshQuery.h" +#include "Runtime/Interfaces/ITerrainManager.h" + + +HeightMeshQuery::HeightMeshQuery () +: m_HeightMaps (NULL) +, m_VerticalRayOffset (0.0f) +{ +} + +HeightMeshQuery::~HeightMeshQuery () +{ +} + +void HeightMeshQuery::Init (const HeightmapDataVector* heightMaps, float verticalRayOffset) +{ + m_HeightMaps = heightMaps; + m_VerticalRayOffset = verticalRayOffset; +} + +dtStatus HeightMeshQuery::getHeight (const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const +{ + Vector3f rayStart (pos[0], pos[1] + m_VerticalRayOffset, pos[2]); + float geometryHeight, terrainHeight; + bool bGeometryHit = GetGeometryHeight (tile, poly, rayStart, &geometryHeight); + bool bTerrainHit = GetTerrainHeight (rayStart, &terrainHeight); + + if (bGeometryHit && bTerrainHit) + { + float geomDist = Abs (rayStart.y - geometryHeight); + float terrainDist = Abs (rayStart.y - terrainHeight); + if (geomDist < terrainDist) + (*height) = geometryHeight; + else + (*height) = terrainHeight; + + return DT_SUCCESS; + } + else if (bGeometryHit) + { + (*height) = geometryHeight; + return DT_SUCCESS; + } + else if (bTerrainHit) + { + (*height) = terrainHeight; + return DT_SUCCESS; + } + else + { + (*height) = pos[1]; + return DT_FAILURE; + } +} + + +// HeightMesh Intersection +// Find the height of the nearest triangle with the same 2D values. +bool HeightMeshQuery::GetGeometryHeight (const dtMeshTile* tile, const dtPoly* poly, const Vector3f& pos, float* height) const +{ + Assert (poly->getType () == DT_POLYTYPE_GROUND); + Assert (height != NULL); + *height = pos.y; + + bool hit = false; + float bestHeight = std::numeric_limits<float>::infinity (); + + const unsigned int ip = (unsigned int)(poly - tile->polys); + const dtPolyDetail* pd = &tile->detailMeshes[ip]; + for (int j = 0; j < pd->triCount; ++j) + { + const dtPolyDetailIndex* t = &tile->detailTris[(pd->triBase+j)*4]; + const float* v[3]; + for (int k = 0; k < 3; ++k) + { + if (t[k] < poly->vertCount) + v[k] = &tile->verts[poly->verts[t[k]]*3]; + else + v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; + } + float h; + if (dtClosestHeightPointTriangle (pos.GetPtr (), v[0], v[1], v[2], h)) + { + if (Abs (pos.y - h) < Abs (pos.y - bestHeight)) + { + *height = h; + bestHeight = h; + hit = true; + } + } + } + + return hit; +} + +bool HeightMeshQuery::GetTerrainHeight (const Vector3f& rayStart, float* height) const +{ + Assert (height != NULL); + *height = rayStart.y; + + if (!m_HeightMaps) + return false; + + ITerrainManager* terrain = GetITerrainManager (); + if (terrain == NULL) + return false; + + bool hit = false; + const float upperBound = rayStart.y; + float lowerBound = -std::numeric_limits<float>::infinity (); + + + for (int i = 0; i < m_HeightMaps->size (); ++i) + { + const HeightmapData& heightmapData = (*m_HeightMaps)[i]; + float terrainHeight; + + // TODO: this should be cleaned up. Abstracting the Terrain-manager away only to feed it + // (serialized!) PPtr's to terrain data is a half-assed abstraction at best. + Object* terrainData = InstanceIDToObjectThreadSafe (heightmapData.terrainData.GetInstanceID ()); + bool hitTerrain = terrain->GetInterpolatedHeight (terrainData, heightmapData.position, rayStart, terrainHeight); + if (!hitTerrain) + continue; + + if (terrainHeight < upperBound && terrainHeight > lowerBound) + { + (*height) = terrainHeight; + lowerBound = terrainHeight; + hit = true; + } + } + return hit; +} diff --git a/Runtime/NavMesh/HeightMeshQuery.h b/Runtime/NavMesh/HeightMeshQuery.h new file mode 100644 index 0000000..1654ba2 --- /dev/null +++ b/Runtime/NavMesh/HeightMeshQuery.h @@ -0,0 +1,28 @@ +#ifndef RUNTIME_HEIGHT_MESH_QUERY +#define RUNTIME_HEIGHT_MESH_QUERY + +#include "External/Recast/Detour/Include/DetourNavMeshQuery.h" +#include "HeightmapData.h" + +class dtNavMeshQuery; +class Vector3f; + +// Query specialization for Height Placement of a HeightMesh. +class HeightMeshQuery : public dtHeightQuery +{ +public: + HeightMeshQuery (); + virtual ~HeightMeshQuery (); + + void Init (const HeightmapDataVector* heightMaps, float verticalRayOffset); + virtual dtStatus getHeight (const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const; + bool GetTerrainHeight (const Vector3f& position, float* height) const; + +private: + bool GetGeometryHeight (const dtMeshTile* tile, const dtPoly* poly, const Vector3f& pos, float* height) const; + + const HeightmapDataVector* m_HeightMaps; + float m_VerticalRayOffset; +}; + +#endif diff --git a/Runtime/NavMesh/HeightmapData.h b/Runtime/NavMesh/HeightmapData.h new file mode 100644 index 0000000..a6cd65d --- /dev/null +++ b/Runtime/NavMesh/HeightmapData.h @@ -0,0 +1,26 @@ +#ifndef HEIGHTMAPDATA_H +#define HEIGHTMAPDATA_H + +#include "Runtime/BaseClasses/BaseObject.h" +#include "Runtime/Serialize/SerializeUtility.h" +#include "Runtime/Math/Vector3.h" +#include "Runtime/Utilities/dynamic_array.h" + +struct HeightmapData +{ + DECLARE_SERIALIZE (HeightmapData) + + Vector3f position; + PPtr<Object> terrainData; +}; + +typedef dynamic_array<HeightmapData> HeightmapDataVector; + +template<class TransferFunction> +void HeightmapData::Transfer (TransferFunction& transfer) +{ + TRANSFER (position); + TRANSFER (terrainData); +} + +#endif 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 (©[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; +} diff --git a/Runtime/NavMesh/NavMesh.h b/Runtime/NavMesh/NavMesh.h new file mode 100644 index 0000000..162f80e --- /dev/null +++ b/Runtime/NavMesh/NavMesh.h @@ -0,0 +1,110 @@ +#pragma once +#include "Runtime/BaseClasses/NamedObject.h" +#include "Runtime/Utilities/dynamic_array.h" +#include "Runtime/Math/Vector3.h" +#include "NavMeshTypes.h" +#include "HeightmapData.h" + + +class dtNavMesh; +class dtNavMeshQuery; +class dtQueryFilter; +class NavMeshPath; +class HeightMeshQuery; + +class NavMesh : public NamedObject +{ +public: + + struct Triangulation + { + dynamic_array<SInt32> layers; + dynamic_array<SInt32> indices; + dynamic_array<Vector3f> vertices; + }; + + REGISTER_DERIVED_CLASS (NavMesh, NamedObject); + DECLARE_OBJECT_SERIALIZE (NavMesh); + + NavMesh (MemLabelId& label, ObjectCreationMode mode); + void Create (); + + bool Raycast (NavMeshHit* hit, const Vector3f& sourcePosition, const Vector3f& targetPosition, const dtQueryFilter& filter); + bool DistanceToEdge (NavMeshHit* hit, const Vector3f& sourcePosition, const dtQueryFilter& filter); + bool SamplePosition (NavMeshHit* hit, const Vector3f& sourcePosition, const dtQueryFilter& filter, float maxDistance); + + int CalculatePolygonPath (NavMeshPath* path, const Vector3f& sourcePosition, const Vector3f& targetPosition, const dtQueryFilter& filter); + int CalculatePathCorners (Vector3f* corners, int maxCorners, const NavMeshPath& path); + + void Triangulate (NavMesh::Triangulation& triangulation) const; + + virtual void AwakeFromLoad (AwakeFromLoadMode mode); + + void SetData (const UInt8* data, unsigned size); + + inline const UInt8* GetMeshData () const; + inline size_t GetMeshDataSize () const; + inline const HeightmapDataVector& GetHeightmaps () const; + inline void SetHeightmaps (HeightmapDataVector& heightmaps); + inline const dtNavMesh* GetInternalNavMesh () const; + inline dtNavMesh* GetInternalNavMesh (); + inline dtNavMeshQuery* GetInternalNavMeshQuery (); + inline const HeightMeshQuery* GetHeightMeshQuery () const; + +private: + bool MapPosition (dtPolyRef* mappedPolyRef, Vector3f* mappedPosition, const Vector3f& position, const Vector3f& extents, const dtQueryFilter& filter) const; + + void Cleanup (); + void CleanupWithError (); + + Vector3f m_QueryExtents; + dynamic_array<UInt8> m_MeshData; + HeightmapDataVector m_Heightmaps; + + dtNavMesh* m_NavMesh; + dtNavMeshQuery* m_NavMeshQuery; + HeightMeshQuery* m_HeightMeshQuery; +}; + +inline const UInt8* NavMesh::GetMeshData () const +{ + return m_MeshData.begin (); +} + +inline size_t NavMesh::GetMeshDataSize () const +{ + return m_MeshData.size (); +} + +inline const HeightmapDataVector& NavMesh::GetHeightmaps () const +{ + return m_Heightmaps; +} + +inline void NavMesh::SetHeightmaps (HeightmapDataVector& heightmaps) +{ + m_Heightmaps = heightmaps; +} + + +inline const dtNavMesh* NavMesh::GetInternalNavMesh () const +{ + return m_NavMesh; +} + +inline dtNavMesh* NavMesh::GetInternalNavMesh () +{ + return m_NavMesh; +} + +inline dtNavMeshQuery* NavMesh::GetInternalNavMeshQuery () +{ + return m_NavMeshQuery; +} + +inline const HeightMeshQuery* NavMesh::GetHeightMeshQuery () const +{ + return m_HeightMeshQuery; +} + +void InvalidateNavMeshHit (NavMeshHit* hit); diff --git a/Runtime/NavMesh/NavMeshAgent.cpp b/Runtime/NavMesh/NavMeshAgent.cpp new file mode 100644 index 0000000..5114554 --- /dev/null +++ b/Runtime/NavMesh/NavMeshAgent.cpp @@ -0,0 +1,1129 @@ +#include "UnityPrefix.h" +#include "NavMeshAgent.h" + +#include "DetourCommon.h" +#include "DetourCrowd.h" +#include "DetourCrowdTypes.h" +#include "NavMesh.h" +#include "NavMeshManager.h" +#include "NavMeshLayers.h" +#include "NavMeshPath.h" +#include "NavMeshSettings.h" +#include "OffMeshLink.h" +#include "Runtime/BaseClasses/MessageHandler.h" +#include "Runtime/BaseClasses/IsPlaying.h" +#include "Runtime/Input/TimeManager.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Filters/AABBUtility.h" +#include "Runtime/Utilities/ValidateArgs.h" +#include "Runtime/Misc/BuildSettings.h" + +#define REQUIRE_INITIALIZED(FUNC, returnValue) \ +if (!InCrowdSystem ()) \ +{ \ + ErrorString (#FUNC " can only be called on an active agent that has been placed on a NavMesh."); \ + return returnValue; \ +} + +NavMeshAgent::NavMeshAgent (MemLabelId& label, ObjectCreationMode mode) +: Super (label, mode) +{ + m_ManagerHandle = -1; + m_CachedPolyRef = -1; + m_Destination = Vector3f::infinityVec; + m_RequestedDestination = Vector3f::infinityVec; + m_StopDistance = false; + m_StopExplicit = false; + m_Request = false; + Reset (); +} + +void NavMeshAgent::Reset () +{ + Super::Reset (); + m_Radius = 0.5F; + m_Height = 2.0F; + m_BaseOffset = 0.0F; + m_Acceleration = 8.0f; + m_AngularSpeed = 120.0F; + m_Speed = 3.5f; + m_AvoidancePriority = 50; + m_ObstacleAvoidanceType = kHighQualityObstacleAvoidance; + m_UpdatePosition = true; + m_UpdateRotation = true; + m_StopRotating = false; + m_AutoTraverseOffMeshLink = true; + m_AutoBraking = true; + m_AutoRepath = true; + m_WalkableMask = 0xFFFFFFFF; + m_StoppingDistance = 0.0f; + m_InstanceID = 0; +} + +void NavMeshAgent::SmartReset () +{ + Super::SmartReset (); + AABB aabb; + if (GetGameObjectPtr () && CalculateLocalAABB (GetGameObject (), &aabb)) + { + Vector3f extents = aabb.GetCenter () + aabb.GetExtent (); + SetRadius (max (extents.x, extents.z)); + SetHeight (2.0F*extents.y); + SetBaseOffset (extents.y); + } + else + { + SetRadius (0.5F); + SetHeight (2.0F); + SetBaseOffset (0.0F); + } +} + +NavMeshAgent::~NavMeshAgent () +{ +} + +template<class TransferFunc> +void NavMeshAgent::Transfer (TransferFunc& transfer) +{ + Super::Transfer (transfer); + + TRANSFER (m_Radius); + TRANSFER (m_Speed); + TRANSFER (m_Acceleration); + transfer.Transfer (m_AvoidancePriority, "avoidancePriority"); + TRANSFER (m_AngularSpeed); + TRANSFER (m_StoppingDistance); + TRANSFER (m_AutoTraverseOffMeshLink); + TRANSFER (m_AutoBraking); + TRANSFER (m_AutoRepath); + transfer.Align (); + TRANSFER (m_Height); + TRANSFER (m_BaseOffset); + TRANSFER (m_WalkableMask); + TRANSFER_ENUM(m_ObstacleAvoidanceType); +} + +void NavMeshAgent::InitializeClass () +{ + REGISTER_MESSAGE (NavMeshAgent, kTransformChanged, OnTransformChanged, int); +} + +void NavMeshAgent::OnTransformChanged (int mask) +{ + if (!InCrowdSystem ()) + return; + + if (mask & Transform::kPositionChanged) + { + const Vector3f npos = GetGroundPositionFromTransform (); + GetCrowdSystem ()->updateAgentPosition (m_AgentHandle, npos.GetPtr ()); + } + if (mask & Transform::kRotationChanged) + m_Angle = std::numeric_limits<float>::infinity (); + if (mask & Transform::kScaleChanged) + UpdateActiveAgentParameters (); +} + +int NavMeshAgent::GetCurrentPolygonMask () const +{ + dtPolyRef polyRef; + if (IsOnOffMeshLink ()) + polyRef = GetCrowdSystem ()->getAgentAnimation (m_AgentHandle)->polyRef; + else + polyRef = GetInternalAgent ()->corridor.getFirstPoly (); + + const dtNavMeshQuery* meshq = GetInternalNavMeshQuery (); + return meshq->getPolygonFlags (polyRef); +} + +const dtQueryFilter& NavMeshAgent::GetFilter () const +{ + Assert (InCrowdSystem ()); + const dtCrowd* crowd = GetCrowdSystem (); + return *crowd->getAgentFilter (m_AgentHandle); +} + +const dtCrowdAgent* NavMeshAgent::GetInternalAgent () const +{ + Assert (InCrowdSystem ()); + return GetCrowdSystem ()->getAgent (m_AgentHandle); +} + +bool NavMeshAgent::SetDestination (const Vector3f& targetPos) +{ + REQUIRE_INITIALIZED ("SetDestination", false); + m_RequestedDestination = targetPos; + m_Request = true; + + dtCrowd* crowd = GetCrowdSystem (); + const dtQueryFilter& filter = GetFilter (); + const dtNavMeshQuery* query = crowd->getNavMeshQuery (); + const float* ext = crowd->getQueryExtents (); + + dtPolyRef newPoly; + float destination[3]; + + query->findNearestPoly (targetPos.GetPtr (), ext, &filter, &newPoly, destination); + + if (!newPoly) + return false; + + if (m_CachedPolyRef == -1 || m_CachedPolyRef != newPoly || IsPathStale ()) + { + if (crowd->requestMoveTarget (m_AgentHandle, newPoly, destination)) + { + m_CachedPolyRef = newPoly; + m_Destination = Vector3f (destination); + if (m_StopExplicit) + { + m_StopExplicit = false; + SetUpdatePosition (true); + } + return true; + } + } + else + { + if (crowd->adjustMoveTarget (m_AgentHandle, m_CachedPolyRef, destination)) + { + m_Destination = Vector3f (destination); + if (m_StopExplicit) + { + m_StopExplicit = false; + SetUpdatePosition (true); + } + return true; + } + } + return false; +} + +Vector3f NavMeshAgent::GetDestination () const +{ + if (HasPath () && !PathPending ()) + return GetEndPositionOfCurrentPath (); + + return m_Destination; +} + +Vector3f NavMeshAgent::GetEndPositionOfCurrentPath () const +{ + Assert (InCrowdSystem ()); + + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f endPosition (agent->corridor.getTarget ()); + return endPosition; +} + +void NavMeshAgent::SetInternalAgentPosition (const Vector3f& position) +{ + if (!InCrowdSystem ()) + return; + + const Transform& transform = GetComponent (Transform); + Vector3f groundPosition = transform.TransformPointWithLocalOffset (position, Vector3f (0.0f, m_BaseOffset, 0.0f)); + GetCrowdSystem ()->moveAgent (m_AgentHandle, groundPosition.GetPtr ()); +} + +Vector3f NavMeshAgent::GetVelocity () const +{ + if (!InCrowdSystem ()) + return Vector3f (0,0,0); + + return Vector3f (GetInternalAgent ()->avel); +} + +void NavMeshAgent::SetVelocity (const Vector3f& vel) +{ + if (!InCrowdSystem ()) + return; + + ABORT_INVALID_VECTOR3 (vel, velocity, navmeshagent); + GetCrowdSystem ()->updateAgentVelocity (m_AgentHandle, vel.GetPtr ()); +} + +Vector3f NavMeshAgent::GetNextPosition () const +{ + const Transform& transform = GetComponent (Transform); + if (!InCrowdSystem ()) + return transform.GetPosition (); + + const Vector3f position (GetInternalAgent ()->npos); + return transform.TransformPointWithLocalOffset (position, Vector3f (0.0f, -m_BaseOffset, 0.0f)); +} + +Vector3f NavMeshAgent::GetNextCorner () const +{ + if (!InCrowdSystem ()) + return GetComponent (Transform).GetPosition (); + + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f target; + GetCrowdSystem ()->getSteerTarget (target.GetPtr (), agent); + return target; +} + +Vector3f NavMeshAgent::GetDesiredVelocity () const +{ + if (!InCrowdSystem ()) + return Vector3f::zero; + + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f desiredVelocity (agent->nvel); + return desiredVelocity; +} + +bool NavMeshAgent::IsOnOffMeshLink () const +{ + if (!InCrowdSystem ()) + return false; + + const dtCrowdAgent* agent = GetInternalAgent (); + return agent->state == DT_CROWDAGENT_STATE_OFFMESH; +} + +// @TODO: Deprecate (de)activation of baked offmeshlinks. +// We really don't want to be holding an OffMeshLink instance ID here. +// Instead promote use of: NavMeshAgent.currentOffMeshLinkData.offMeshLink +void NavMeshAgent::ActivateCurrentOffMeshLink (bool activated) +{ + if (!IsOnOffMeshLink ()) + return; + + int instanceID = 0; + if (!activated) + { + const dtPolyRef polyref = GetCrowdSystem ()->getAgentAnimation (m_AgentHandle)->polyRef; + GetNavMeshManager ().GetInternalNavMesh ()->getOffMeshLinkInstanceIDByRef (polyref, &instanceID); + m_InstanceID = instanceID; + } + else + { + instanceID = m_InstanceID; + m_InstanceID = 0; + } + + if (OffMeshLink* oml = dynamic_instanceID_cast<OffMeshLink*> (instanceID)) + { + oml->SetActivated (activated); + } + else + { + const dtPolyRef polyref = GetCrowdSystem ()->getAgentAnimation (m_AgentHandle)->polyRef; + GetNavMeshSettings ().SetOffMeshPolyAccess (polyref, activated); + } +} + + +bool NavMeshAgent::GetCurrentOffMeshLinkData (OffMeshLinkData* data) const +{ + Assert (data); + memset (data, 0, sizeof (OffMeshLinkData)); + if (!IsOnOffMeshLink ()) + return false; + + const dtCrowdAgentAnimation* agentAnimation = GetCrowdSystem ()->getAgentAnimation (m_AgentHandle); + if (agentAnimation == NULL) + return false; + + if (!SetOffMeshLinkDataFlags (data, agentAnimation->polyRef)) + return false; + + data->m_StartPos = Vector3f (agentAnimation->startPos); + data->m_EndPos = Vector3f (agentAnimation->endPos); + return true; +} + +bool NavMeshAgent::GetNextOffMeshLinkData (OffMeshLinkData* data) const +{ + Assert (data); + memset (data, 0, sizeof (OffMeshLinkData)); + if (!InCrowdSystem ()) + return false; + + const dtPathCorridor& corridor = GetInternalAgent ()->corridor; + if (!corridor.isPathValid ()) + return false; + + const dtNavMesh* navmesh = GetNavMeshManager ().GetInternalNavMesh (); + const dtPolyRef* pathPolygons = corridor.getPath (); + const int pathCount = corridor.getPathCount (); + int i = 1; + for (; i < pathCount; ++i) + { + if (SetOffMeshLinkDataFlags (data, pathPolygons[i])) + break; + } + + // Note: We intentionally fail if pathCount == 1 + // in that case any offmeshlink is not 'next' - but current. + if (i >= pathCount) + return false; + + dtStatus status = navmesh->getOffMeshConnectionPolyEndPoints (pathPolygons[i-1], pathPolygons[i], data->m_StartPos.GetPtr (), data->m_EndPos.GetPtr ()); + + // Having successfully called 'SetOffMeshLinkDataFlags' above should ensure valid endpoints here. + DebugAssert (status == DT_SUCCESS); + if (status != DT_SUCCESS) + { + memset (data, 0, sizeof (OffMeshLinkData)); + return false; + } + + return true; +} + +// Set OffMeshLink data or return false +// Returns false if polyRef is not an offmeshlink +bool NavMeshAgent::SetOffMeshLinkDataFlags (OffMeshLinkData* data, const dtPolyRef polyRef) const +{ + Assert (InCrowdSystem ()); + + const dtNavMesh* navmesh = GetNavMeshManager ().GetInternalNavMesh (); + + int instanceID, linkType, activated; + if (!navmesh->GetOffMeshLinkData (polyRef, &instanceID, &linkType, &activated)) + return false; + + data->m_Valid = 1; + data->m_Activated = activated; + data->m_LinkType = static_cast<OffMeshLinkType> (linkType); + data->m_InstanceID = instanceID; + + return true; +} + +void NavMeshAgent::SetUpdatePosition (bool inbool) +{ + if (m_UpdatePosition == inbool) + return; + + m_UpdatePosition = inbool; + if (inbool) + { + Vector3f npos = GetGroundPositionFromTransform (); + GetCrowdSystem ()->updateAgentPosition (m_AgentHandle, npos.GetPtr ()); + } +} + +void NavMeshAgent::SetUpdateRotation (bool inbool) +{ + m_UpdateRotation = inbool; + if (inbool) + { + const Transform& transform = GetComponent (Transform); + Vector3f euler = QuaternionToEuler (transform.GetRotation ()); + m_Angle = euler.y; + } +} + +void NavMeshAgent::SetAutoTraverseOffMeshLink (bool inbool) +{ + m_AutoTraverseOffMeshLink = inbool; + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetAutoBraking (bool inbool) +{ + m_AutoBraking = inbool; + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetAutoRepath (bool inbool) +{ + m_AutoRepath = inbool; + SetDirty (); +} + +void NavMeshAgent::UpdateActiveAgentParameters () +{ + CheckConsistency (); + if (InCrowdSystem ()) + { + dtCrowdAgentParams params; + FillAgentParams (params); + GetCrowdSystem ()->updateAgentParameters (m_AgentHandle, ¶ms); + } +} + +void NavMeshAgent::AwakeFromLoad (AwakeFromLoadMode mode) +{ + Super::AwakeFromLoad (mode); + UpdateActiveAgentParameters (); +} + +void NavMeshAgent::CheckConsistency () +{ + Super::CheckConsistency (); + m_AvoidancePriority = clamp (m_AvoidancePriority, 0, 99); + m_Speed = clamp (m_Speed, 0.0f, 1e15f); // squaring m_Speed keeps it below inf. + m_StoppingDistance = max (0.0f, m_StoppingDistance); + m_Acceleration = max (0.0f, m_Acceleration); + m_AngularSpeed = max (0.0f, m_AngularSpeed); + m_Height = EnsurePositive (m_Height); + m_Radius = EnsurePositive (m_Radius); +} + +void NavMeshAgent::UpdateState () +{ + if (!InCrowdSystem ()) + return; + + const float remainingDistance = GetRemainingDistance (); + StopOrResume (remainingDistance); + + if (m_AutoRepath && m_Request) + RepathIfStuck (remainingDistance); + + // Previously we were resetting the path when agentes reached the stopping distance. + // Now this is no longer done, the path is always kept. We can kill this code on the next backwards breaking release. + if (!IS_CONTENT_NEWER_OR_SAME (kUnityVersion4_0_a1)) + { + if (m_StopDistance && !PathPending ()) + ResetPath (); + } +} + +void NavMeshAgent::StopOrResume (float remainingDistance) +{ + m_StopDistance = remainingDistance < m_StoppingDistance && HasPath (); + if (!m_StopExplicit) + { + m_StopRotating = false; + } + + if (m_StopExplicit || m_StopDistance) + GetCrowdSystem ()->stopAgent (m_AgentHandle); + else + GetCrowdSystem ()->resumeAgent (m_AgentHandle); +} + +void NavMeshAgent::RepathIfStuck (float remainingDistance) +{ + bool isValid = IsPathValid (); + bool isOffMesh = IsOnOffMeshLink (); + bool isPartial = IsPathPartial (); + bool isStale = IsPathStale (); + + // m_Radius must be scaled by transform scale - so use the (already scaled) internal radius! + const float agentRadius = GetInternalAgent ()->params.radius; + if (!isValid || (!isOffMesh && isPartial && isStale && remainingDistance <= agentRadius)) + { + m_CachedPolyRef = -1; + SetDestination (m_RequestedDestination); + } +} + +static inline void SetTransformMessageEnabled (bool enable) +{ + GameObject::GetMessageHandler ().SetMessageEnabled (ClassID (NavMeshAgent), kTransformChanged.messageID, enable); +} + +void NavMeshAgent::UpdateTransform (float deltaTime) +{ + if (!InCrowdSystem ()) + return; + + Transform& transform = GetComponent (Transform); + + // Avoid dirtying the transform twice (when updating position and rotation) + // Instead call "WithoutNotification" variants of the SetPosition / SetRotation + // and signal change once at the end using 'changeMessageMask' + int changeMessageMask = 0; + + if (m_UpdatePosition) + { + const Vector3f groundPosition = Vector3f (GetInternalAgent ()->npos); + const Vector3f localOffset = Vector3f (0.0f, -m_BaseOffset, 0.0f); + const Vector3f newPosition = transform.TransformPointWithLocalOffset (groundPosition, localOffset); + transform.SetPositionWithoutNotification (newPosition); + changeMessageMask |= Transform::kPositionChanged; + } + + if (m_UpdateRotation && !m_StopRotating) + { + UpdateRotation (transform, deltaTime); + changeMessageMask |= Transform::kRotationChanged; + } + + if (changeMessageMask) + { + // Update the transform hierarchy w/o triggering our own callback + SetTransformMessageEnabled (false); + transform.SendTransformChanged (changeMessageMask); + SetTransformMessageEnabled (true); + } +} + +void NavMeshAgent::UpdateRotation (Transform& transform, float deltaTime) +{ + if (m_Angle == std::numeric_limits<float>::infinity ()) + { + Vector3f euler = QuaternionToEuler (transform.GetRotation ()); + m_Angle = euler.y; + } + + const dtCrowdAgent* agent = GetInternalAgent (); + float sqrLen = Sqr (agent->vel[0]) + Sqr (agent->vel[2]); + if (sqrLen > 0.001F) + { + float angle = atan2 (agent->vel[0], agent->vel[2]); + const float deltaAngle = DeltaAngleRad (m_Angle, angle); + const float maxAngleSpeed = std::min (Abs (deltaAngle)*(1.0f+sqrtf (sqrLen)), Deg2Rad (m_AngularSpeed)); + m_Angle += std::min (Abs (deltaAngle), maxAngleSpeed*deltaTime) * Sign (deltaAngle); + } + Quaternionf rotation = AxisAngleToQuaternion (Vector3f::yAxis, m_Angle); + transform.SetRotationWithoutNotification (rotation); +} + +void NavMeshAgent::FillAgentParams (dtCrowdAgentParams& params) +{ + Vector2f scale = CalculateDimensionScales (); + params.radius = max (0.00001F, m_Radius*scale.x); + params.height = max (0.00001F, m_Height*scale.y); + params.maxAcceleration = m_Acceleration; + params.maxSpeed = m_Speed; + params.priority = 99 - m_AvoidancePriority; + params.obstacleAvoidanceType = m_ObstacleAvoidanceType; + params.includeFlags = GetWalkableMask (); + + params.updateFlags = 0; + if (m_ObstacleAvoidanceType != kNoObstacleAvoidance) + params.updateFlags |= DT_CROWD_OBSTACLE_AVOIDANCE; + if (m_AutoTraverseOffMeshLink) + params.updateFlags |= DT_CROWD_AUTO_TRAVERSE_OFFMESHLINK; + if (m_AutoBraking) + params.updateFlags |= DT_CROWD_AUTO_BRAKING; +} + +void NavMeshAgent::OnNavMeshChanged () +{ + if (InCrowdSystem ()) + { + ReinstateInCrowdSystem (); + } + else + { + AddToCrowdSystem (); + } +} + +void NavMeshAgent::AddToCrowdSystem () +{ + if (!IsWorldPlaying ()) + return; + + if (GetInternalNavMeshQuery () == NULL) + { + WarningString ("Failed to create agent because there is no valid NavMesh"); + return; + } + dtCrowd* crowd = GetCrowdSystem (); + Assert (crowd != NULL); + + dtCrowdAgentParams params; + FillAgentParams (params); + + Assert (!InCrowdSystem ()); + Assert (m_CachedPolyRef == -1); + Vector3f currentPosition = GetGroundPositionFromTransform (); + if (!crowd->addAgent (m_AgentHandle, currentPosition.GetPtr (), ¶ms)) + { + WarningStringObject ("Failed to create agent because it is not close enough to the NavMesh", this); + return; + } + + const dtCrowdAgent* agent = GetInternalAgent (); + + m_CachedPolyRef = agent->corridor.getFirstPoly (); + m_Destination = Vector3f (agent->corridor.getPos ()); + m_RequestedDestination = m_Destination; + m_Angle = std::numeric_limits<float>::infinity (); + + // Collect global layer costs and apply to created agent + float layerCosts[NavMeshLayers::kLayerCount]; + NavMeshLayers& layers = GetNavMeshLayers (); + for (int il = 0; il < NavMeshLayers::kLayerCount; ++il) + { + layerCosts[il] = layers.GetLayerCost (il); + } + crowd->initializeAgentFilter (m_AgentHandle, layerCosts, NavMeshLayers::kLayerCount); +} + +void NavMeshAgent::ReinstateInCrowdSystem () +{ + if (!InCrowdSystem ()) + return; + + dtCrowd* crowd = GetCrowdSystem (); + Assert (crowd); + + ResetCachedPolyRef (); + if (!crowd->remapAgentPathStart (m_AgentHandle)) + { + RemoveFromCrowdSystem (); + } + else if (m_AutoRepath && m_Request) + { + SetDestination (m_RequestedDestination); + } +} + +void NavMeshAgent::RemoveFromCrowdSystem () +{ + if (!InCrowdSystem ()) + return; + +#if UNITY_EDITOR + dtCrowdAgentDebugInfo* debugInfo = GetNavMeshManager ().GetInternalDebugInfo (); + if (debugInfo && debugInfo->idx == m_AgentHandle.GetIndex ()) + { + debugInfo->idx = -1; + } +#endif + + GetCrowdSystem ()->removeAgent (m_AgentHandle); + m_CachedPolyRef = -1; +} + +void NavMeshAgent::AddToManager () +{ + GetNavMeshManager ().RegisterAgent (*this, m_ManagerHandle); + AddToCrowdSystem (); +} + +void NavMeshAgent::RemoveFromManager () +{ + RemoveFromCrowdSystem (); + GetNavMeshManager ().UnregisterAgent (m_ManagerHandle); +} + +float NavMeshAgent::GetRemainingDistance () const +{ + REQUIRE_INITIALIZED ("GetRemainingDistance", std::numeric_limits<float>::infinity ()); + return GetCrowdSystem ()->CalculateRemainingPath (m_AgentHandle); +} + +int NavMeshAgent::CalculatePolygonPath (const Vector3f& targetPosition, NavMeshPath* path) +{ + REQUIRE_INITIALIZED ("CalculatePolygonPath", 0) + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f sourcePosition (agent->corridor.getPos ()); + + const dtQueryFilter& filter = GetFilter (); + return GetNavMeshSettings ().GetNavMesh ()->CalculatePolygonPath (path, sourcePosition, targetPosition, filter); +} + +bool NavMeshAgent::SetPath (const NavMeshPath* path) +{ + REQUIRE_INITIALIZED ("SetPath", false) + + ResetPath (); + + const NavMeshPathStatus status = path->GetStatus (); + const int polyCount = path->GetPolygonCount (); + if (status == kPathInvalid || polyCount == 0) + return false; + + const Vector3f targetPos = path->GetTargetPosition (); + const Vector3f sourcePos = path->GetSourcePosition (); + const dtPolyRef* polyPath = path->GetPolygonPath (); + GetCrowdSystem ()->setAgentPath (m_AgentHandle, sourcePos.GetPtr (), targetPos.GetPtr (), polyPath, polyCount, status == kPathPartial); + const dtCrowdAgent* agent = GetInternalAgent (); + + dtPolyRef lastPoly = agent->corridor.getLastPoly (); + if (lastPoly != polyPath[polyCount - 1]) + return false; + + m_CachedPolyRef = lastPoly; + return true; +} + +void NavMeshAgent::CopyPath (NavMeshPath* path) const +{ + Assert (path); + if (!m_AgentHandle.IsValid ()) + { + path->SetPolygonCount (0); + path->SetStatus (kPathInvalid); + return; + } + + const dtCrowdAgent* agent = GetInternalAgent (); + + unsigned int pathCount = agent->corridor.getPathCount (); + memcpy (path->GetPolygonPath (), agent->corridor.getPath (), sizeof (dtPolyRef)*pathCount); + path->SetPolygonCount (pathCount); + path->SetTargetPosition (Vector3f (agent->corridor.getTarget ())); + path->SetSourcePosition (Vector3f (agent->corridor.getPos ())); + path->SetStatus (GetPathStatus ()); +} + +void NavMeshAgent::ResetPath () +{ + REQUIRE_INITIALIZED ("ResetPath",) + + GetCrowdSystem ()->resetAgentPath (m_AgentHandle); + m_StopDistance = false; + m_StopExplicit = false; + m_Request = false; + m_CachedPolyRef = -1; +} + +bool NavMeshAgent::PathPending () const +{ + return InCrowdSystem () && GetInternalAgent ()->pendingRequest; +} + +bool NavMeshAgent::HasPath () const +{ + if (!InCrowdSystem () || m_CachedPolyRef == -1) + return false; + + const dtCrowdAgent* agent = GetInternalAgent (); + if (agent->state == DT_CROWDAGENT_STATE_WALKING) + { + return agent->ncorners > 0; + } + else if (agent->state == DT_CROWDAGENT_STATE_OFFMESH || agent->state == DT_CROWDAGENT_STATE_WAITING_OFFMESH) + { + return true; + } + + return false; +} + +bool NavMeshAgent::DistanceToEdge (NavMeshHit* hit) const +{ + Assert (hit); + REQUIRE_INITIALIZED ("DistanceToEdge", false); + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f sourcePosition (agent->corridor.getPos ()); + + const dtQueryFilter& filter = GetFilter (); + return GetNavMeshSettings ().GetNavMesh ()->DistanceToEdge (hit, sourcePosition, filter); +} + +bool NavMeshAgent::Raycast (const Vector3f& targetPosition, NavMeshHit* hit) +{ + Assert (hit); + REQUIRE_INITIALIZED ("Raycast", false) + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f sourcePosition (agent->corridor.getPos ()); + + const dtQueryFilter& filter = GetFilter (); + return GetNavMeshSettings ().GetNavMesh ()->Raycast (hit, sourcePosition, targetPosition, filter); +} + +// TODO: handle case where: maxDistance > remainingDistance (non-inf). +bool NavMeshAgent::SamplePathPosition (int passableMask, float maxDistance, NavMeshHit* hit) +{ + Assert (hit); + REQUIRE_INITIALIZED ("SamplePathPosition", false); + + const dtCrowdAgent* agent = GetInternalAgent (); + const dtNavMeshQuery* meshq = GetInternalNavMeshQuery (); + Vector3f sourcePosition (agent->corridor.getPos ()); + Vector3f agentPosition (agent->npos); + + // Copy and modify agent filter with provided mask + dtQueryFilter filter = GetFilter (); + filter.setIncludeFlags (passableMask & filter.getIncludeFlags ()); + + if (agent->ncorners == 0 || maxDistance <= 0.0f) + { + hit->position = agentPosition; + hit->normal = Vector3f::zero; + hit->distance = 0.0f; + hit->mask = GetCurrentPolygonMask (); + hit->hit = false; + return false; + } + + if (!meshq->passRef (agent->corridor.getFirstPoly (), &filter)) + { + hit->position = sourcePosition; + hit->normal = Vector3f::zero; + hit->distance = 0.0f; + hit->mask = GetCurrentPolygonMask (); + hit->hit = true; + return true; + } + + float totalDistance = 0.0f; + for (int i = 0; i < agent->ncorners; ++i) + { + const Vector3f targetPosition (&agent->cornerVerts[3*i]); + if (GetNavMeshSettings ().GetNavMesh ()->Raycast (hit, sourcePosition, targetPosition, filter)) + { + const float hitDistance = totalDistance + hit->distance; + if (hitDistance <= maxDistance) + { + // position, normal, mask, hit set by Raycast () + hit->distance = hitDistance; // set to distance along path + return true; + } + } + float segLen = Magnitude (targetPosition-sourcePosition); + if (totalDistance+segLen > maxDistance) + { + // TODO: set proper height in "hit->position[1]" + hit->position = Lerp (sourcePosition, targetPosition, (maxDistance-totalDistance)/segLen); + hit->normal = Vector3f::zero; + hit->distance = maxDistance; + hit->mask = meshq->getPolygonFlags (agent->cornerPolys[i]); + hit->hit = false; + return false; + } + + totalDistance += segLen; + sourcePosition = targetPosition; + } + + // Found end of straight path in "agent->cornerVerts" + // - this is not necessarily the end of the path. + return false; +} + +bool NavMeshAgent::Warp (const Vector3f& newPosition) +{ + RemoveFromCrowdSystem (); + Transform& transform = GetComponent (Transform); + transform.SetPosition (newPosition); + AddToCrowdSystem (); + return InCrowdSystem (); +} + +void NavMeshAgent::Move (const Vector3f& motion) +{ + REQUIRE_INITIALIZED ("Move",) + + const dtCrowdAgent* agent = GetInternalAgent (); + Vector3f targetPos = motion + Vector3f (agent->npos); + GetCrowdSystem ()->moveAgent (m_AgentHandle, targetPos.GetPtr ()); + + if (m_UpdatePosition) + { + SetTransformFromGroundPosition (Vector3f (agent->npos)); + } +} + +void NavMeshAgent::Stop (bool stopUpdates) +{ + REQUIRE_INITIALIZED ("Stop",) + + m_StopExplicit = true; + if (stopUpdates) + { + SetVelocity (Vector3f::zero); + SetUpdatePosition (false); + m_StopRotating = true; + } +} + +void NavMeshAgent::Resume () +{ + REQUIRE_INITIALIZED ("Resume",) + + m_StopExplicit = false; + SetUpdatePosition (true); +} + +void NavMeshAgent::CompleteOffMeshLink () +{ + REQUIRE_INITIALIZED ("CompleteOffMeshLink",) + + GetCrowdSystem ()->completeOffMeshLink (m_AgentHandle); +} + +NavMeshPathStatus NavMeshAgent::GetPathStatus () const +{ + if (!InCrowdSystem () || !GetInternalAgent ()->corridor.isPathValid ()) + return kPathInvalid; + + if (GetInternalAgent ()->corridor.isPathPartial ()) + return kPathPartial; + + return kPathComplete; +} + +bool NavMeshAgent::IsPathValid () const +{ + if (!InCrowdSystem ()) + return false; + + return GetInternalAgent ()->corridor.isPathValid (); +} + +bool NavMeshAgent::IsPathPartial () const +{ + if (!InCrowdSystem ()) + return false; + + return GetInternalAgent ()->corridor.isPathPartial (); +} + +bool NavMeshAgent::IsPathStale () const +{ + if (!InCrowdSystem ()) + return false; + + return GetInternalAgent ()->corridor.isPathStale (); +} + +void NavMeshAgent::SetLayerCost (unsigned int layer, float cost) +{ + REQUIRE_INITIALIZED ("SetLayerCost",) + + if (layer >= NavMeshLayers::kLayerCount) + { + ErrorString ("Index out of bounds"); + return; + } + +#if UNITY_EDITOR + if (cost < 1.0f) + { + WarningStringObject (NavMeshLayers::s_WarningCostLessThanOne, this); + } +#endif + + GetCrowdSystem ()->updateAgentFilterCost (m_AgentHandle, layer, cost); +} + +float NavMeshAgent::GetLayerCost (unsigned int layer) const +{ + REQUIRE_INITIALIZED ("GetLayerCost", 0.0F) + + if (layer >= NavMeshLayers::kLayerCount) + { + ErrorString ("Index out of bounds"); + return 0.0F; + } + + const dtQueryFilter& filter = GetFilter (); + return filter.getAreaCost (layer); +} + +void NavMeshAgent::SetHeight (float height) +{ + ABORT_INVALID_FLOAT (height, height, navmeshagent); + m_Height = EnsurePositive (height); + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetBaseOffset (float baseOffset) +{ + ABORT_INVALID_FLOAT (baseOffset, baseOffset, navmeshagent); + m_BaseOffset = baseOffset; + SetDirty (); + if (!InCrowdSystem ()) + return; + + if (m_UpdatePosition) + { + const dtCrowdAgent* agent = GetInternalAgent (); + SetTransformFromGroundPosition (Vector3f (agent->npos)); + } +} + +Vector2f NavMeshAgent::CalculateDimensionScales () const +{ + Vector3f absScale = Abs (GetComponent (Transform).GetWorldScaleLossy ()); + float scaleWidth = max (absScale.x, absScale.z); + float scaleHeight = absScale.y; + return Vector2f (scaleWidth, scaleHeight); +} + +float NavMeshAgent::CalculateScaledRadius () const +{ + const Vector2f scale = CalculateDimensionScales (); + return m_Radius*scale.x; +} + +float NavMeshAgent::CalculateScaledHeight () const +{ + const Vector2f scale = CalculateDimensionScales (); + return m_Height*scale.y; +} + +void NavMeshAgent::SetRadius (float radius) +{ + ABORT_INVALID_FLOAT (radius, radius, navmeshagent); + m_Radius = EnsurePositive (radius); + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetSpeed (float value) +{ + ABORT_INVALID_FLOAT (value, speed, navmeshagent); + m_Speed = clamp (value, 0.0f, 1e15f); // squaring m_Speed keeps it below inf. + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetAvoidancePriority (int value) +{ + m_AvoidancePriority = value; + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetAngularSpeed (float value) +{ + ABORT_INVALID_FLOAT (value, angularSpeed, navmeshagent); + m_AngularSpeed = value; + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetAcceleration (float value) +{ + ABORT_INVALID_FLOAT (value, acceleration, navmeshagent); + m_Acceleration = value; + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetStoppingDistance (float value) +{ + ABORT_INVALID_FLOAT (value, stoppingDistance, navmeshagent); + m_StoppingDistance = value; + SetDirty (); +} + +void NavMeshAgent::SetWalkableMask (UInt32 mask) +{ + if (m_WalkableMask == mask) + return; + + m_WalkableMask = mask; + + UpdateActiveAgentParameters (); + SetDirty (); +} + +void NavMeshAgent::SetObstacleAvoidanceType (int type) +{ + m_ObstacleAvoidanceType = (ObstacleAvoidanceType) type; + UpdateActiveAgentParameters (); + SetDirty (); +} + +const dtNavMeshQuery* NavMeshAgent::GetInternalNavMeshQuery () +{ + return GetNavMeshManager ().GetInternalNavMeshQuery (); +} + +dtCrowd* NavMeshAgent::GetCrowdSystem () +{ + return GetNavMeshManager ().GetCrowdSystem (); +} + +IMPLEMENT_CLASS_HAS_INIT (NavMeshAgent) +IMPLEMENT_OBJECT_SERIALIZE (NavMeshAgent) diff --git a/Runtime/NavMesh/NavMeshAgent.h b/Runtime/NavMesh/NavMeshAgent.h new file mode 100644 index 0000000..fe5a943 --- /dev/null +++ b/Runtime/NavMesh/NavMeshAgent.h @@ -0,0 +1,313 @@ +#pragma once +#ifndef RUNTIME_NAVMESH_AGENT +#define RUNTIME_NAVMESH_AGENT + +#include "Runtime/GameCode/Behaviour.h" +#include "Runtime/Graphics/Transform.h" +#include "NavMeshTypes.h" +#include "Runtime/Math/Vector2.h" + +class NavMeshPath; +struct OffMeshLinkData; +struct NavMeshHit; + +class dtNavMeshQuery; +class dtCrowd; +class dtQueryFilter; +struct dtCrowdAgentParams; +struct dtCrowdAgent; + + +class NavMeshAgent : public Behaviour +{ +public: + REGISTER_DERIVED_CLASS (NavMeshAgent, Behaviour) + DECLARE_OBJECT_SERIALIZE (NavMeshAgent) + + NavMeshAgent (MemLabelId& label, ObjectCreationMode mode); + // ~NavMeshAgent (); declared by a macro + + static void InitializeClass (); + static void CleanupClass () {} // Avoid double free of Behavior + + virtual void AwakeFromLoad (AwakeFromLoadMode mode); + virtual void CheckConsistency (); + + inline bool InCrowdSystem () const; + inline void SetManagerHandle (int handle); + bool SetDestination (const Vector3f& position); + Vector3f GetDestination () const; + void SetInternalAgentPosition (const Vector3f& position); + + int CalculatePolygonPath (const Vector3f& targetPosition, NavMeshPath* path); + bool SetPath (const NavMeshPath* path); + void CopyPath (NavMeshPath* path) const; + void ResetPath (); + bool PathPending () const; + bool HasPath () const; + bool DistanceToEdge (NavMeshHit* hit) const; + bool Raycast (const Vector3f& position, NavMeshHit* hitPos); + float GetRemainingDistance () const; + + // Consider moving these to the 'NavMeshPath' class + bool SamplePathPosition (int collisionMask, float maxDistance, NavMeshHit* hit); + Vector3f GetEndPositionOfCurrentPath () const; + bool IsPathValid () const; + bool IsPathPartial () const; + bool IsPathStale () const; + NavMeshPathStatus GetPathStatus () const; + inline void ResetCachedPolyRef (); + + UInt32 GetWalkableMask () const { return m_WalkableMask; } + void SetWalkableMask (UInt32 mask); + + void SetLayerCost (unsigned int layer, float cost); + float GetLayerCost (unsigned int layer) const; + + inline ObstacleAvoidanceType GetObstacleAvoidanceType () const; + void SetObstacleAvoidanceType (int type); + + void SetAvoidancePriority (int value); + inline int GetAvoidancePriority () const; + + inline float GetSpeed () const; + void SetSpeed (float value); + + inline float GetAngularSpeed () const; + void SetAngularSpeed (float value); + + inline float GetAcceleration () const; + void SetAcceleration (float value); + + inline float GetStoppingDistance () const; + void SetStoppingDistance (float value); + + Vector3f GetVelocity () const; + void SetVelocity (const Vector3f& vel); + Vector3f GetNextPosition () const; + + Vector3f GetNextCorner () const; + Vector3f GetDesiredVelocity () const; + + bool IsOnOffMeshLink () const; + void ActivateCurrentOffMeshLink (bool activated); + bool GetCurrentOffMeshLinkData (OffMeshLinkData* data) const; + bool GetNextOffMeshLinkData (OffMeshLinkData* data) const; + bool SetOffMeshLinkDataFlags (OffMeshLinkData* data, const dtPolyRef polyRef) const; + + inline bool GetUpdatePosition () const; + void SetUpdatePosition (bool inbool); + inline bool GetUpdateRotation () const; + void SetUpdateRotation (bool inbool); + inline bool GetAutoTraverseOffMeshLink () const; + void SetAutoTraverseOffMeshLink (bool inbool); + inline bool GetAutoBraking () const; + void SetAutoBraking (bool inbool); + inline bool GetAutoRepath () const; + void SetAutoRepath (bool inbool); + + inline float GetRadius () const; + inline float GetHeight () const; + inline float GetBaseOffset () const; + + float CalculateScaledRadius () const; + float CalculateScaledHeight () const; + + void SetRadius (float radius); + void SetHeight (float height); + void SetBaseOffset (float baseOffset); + + bool Warp (const Vector3f& newPosition); + void Move (const Vector3f& motion); + void Stop (bool stopUpdates); + void Resume (); + void CompleteOffMeshLink (); + + void UpdateState (); + void UpdateTransform (float deltaTime); + + inline Vector3f GetGroundPositionFromTransform () const; + + void OnNavMeshChanged (); + inline void OnNavMeshCleanup (); + +protected: + virtual void AddToManager (); + virtual void RemoveFromManager (); + virtual void Reset (); + virtual void SmartReset (); + + void OnTransformChanged (int mask); + +private: + + void AddToCrowdSystem (); + void RemoveFromCrowdSystem (); + void ReinstateInCrowdSystem (); + void StopOrResume (float remainingDistance); + void RepathIfStuck (float remainingDistance); + void UpdateRotation (Transform& transform, float deltaTime); + + void FillAgentParams (dtCrowdAgentParams& params); + void UpdateActiveAgentParameters (); + Vector2f CalculateDimensionScales () const; + + inline float CalculateScaledBaseOffset (const Transform& transform) const; + inline void SetTransformFromGroundPosition (const Vector3f& groundPosition); + + int GetCurrentPolygonMask () const; + const dtQueryFilter& GetFilter () const; + const dtCrowdAgent* GetInternalAgent () const; + static const dtNavMeshQuery* GetInternalNavMeshQuery (); + static dtCrowd* GetCrowdSystem (); + static inline float EnsurePositive (float value); + + Vector3f m_Destination; + Vector3f m_RequestedDestination; + float m_Radius; + float m_Height; + float m_BaseOffset; + float m_Speed; + float m_AngularSpeed; + float m_Angle; + float m_Acceleration; + float m_StoppingDistance; + + // Internal Crowd system agent index + dtCrowdHandle m_AgentHandle; + int m_InstanceID; + // Persistent manager handle + int m_ManagerHandle; + + dtPolyRef m_CachedPolyRef; + + ObstacleAvoidanceType m_ObstacleAvoidanceType; ///< enum { None = 0, Low Quality, Medium Quality, Good Quality, High Quality } + UInt32 m_WalkableMask; + int m_AvoidancePriority; + bool m_AutoTraverseOffMeshLink; + bool m_AutoBraking; + bool m_AutoRepath; + + bool m_UpdatePosition : 1; + bool m_UpdateRotation : 1; + bool m_StopDistance : 1; + bool m_StopExplicit : 1; + bool m_StopRotating : 1; + bool m_Request : 1; + + friend void DrawNavMeshAgent (const NavMeshAgent& agent); +}; + +inline float NavMeshAgent::EnsurePositive (float value) +{ + return std::max (0.00001F, value); +} + +inline Vector3f NavMeshAgent::GetGroundPositionFromTransform () const +{ + const Transform& transform = GetComponent (Transform); + return transform.TransformPoint (Vector3f (0, -m_BaseOffset, 0)); +} + +inline void NavMeshAgent::SetTransformFromGroundPosition (const Vector3f& groundPosition) +{ + Transform& transform = GetComponent (Transform); + transform.SetPositionWithLocalOffset (groundPosition, Vector3f (0.0f, -m_BaseOffset, 0.0f)); +} + +inline float NavMeshAgent::CalculateScaledBaseOffset (const Transform& transform) const +{ + return m_BaseOffset * Abs (transform.GetWorldScaleLossy ().y); +} + +inline bool NavMeshAgent::InCrowdSystem () const +{ + return m_AgentHandle.IsValid (); +} + +inline void NavMeshAgent::SetManagerHandle (int handle) +{ + m_ManagerHandle = handle; +} + +inline void NavMeshAgent::ResetCachedPolyRef () +{ + m_CachedPolyRef = -1; +} + +inline ObstacleAvoidanceType NavMeshAgent::GetObstacleAvoidanceType () const +{ + return m_ObstacleAvoidanceType; +} + +inline int NavMeshAgent::GetAvoidancePriority () const +{ + return m_AvoidancePriority; +} + +inline float NavMeshAgent::GetSpeed () const +{ + return m_Speed; +} + +inline float NavMeshAgent::GetAngularSpeed () const +{ + return m_AngularSpeed; +} + +inline float NavMeshAgent::GetAcceleration () const +{ + return m_Acceleration; +} + +inline float NavMeshAgent::GetStoppingDistance () const +{ + return m_StoppingDistance; +} + +inline bool NavMeshAgent::GetUpdatePosition () const +{ + return m_UpdatePosition; +} + +inline bool NavMeshAgent::GetUpdateRotation () const +{ + return m_UpdateRotation; +} + +inline bool NavMeshAgent::GetAutoTraverseOffMeshLink () const +{ + return m_AutoTraverseOffMeshLink; +} + +inline bool NavMeshAgent::GetAutoBraking () const +{ + return m_AutoBraking; +} + +inline bool NavMeshAgent::GetAutoRepath () const +{ + return m_AutoRepath; +} + +inline float NavMeshAgent::GetRadius () const +{ + return m_Radius; +} + +inline float NavMeshAgent::GetHeight () const +{ + return m_Height; +} + +inline float NavMeshAgent::GetBaseOffset () const +{ + return m_BaseOffset; +} + +inline void NavMeshAgent::OnNavMeshCleanup () +{ + RemoveFromCrowdSystem (); +} + +#endif diff --git a/Runtime/NavMesh/NavMeshCarving.cpp b/Runtime/NavMesh/NavMeshCarving.cpp new file mode 100644 index 0000000..d437bf7 --- /dev/null +++ b/Runtime/NavMesh/NavMeshCarving.cpp @@ -0,0 +1,206 @@ +#include "UnityPrefix.h" +#include "NavMeshCarving.h" + +#include "DetourNavMesh.h" +#include "NavMesh.h" +#include "NavMeshSettings.h" +#include "DetourCrowdTypes.h" +#include "NavMeshObstacle.h" +#include "NavMeshProfiler.h" +#include <float.h> + +// Performance note: +// The performance of the current implementation will be sub-optimal +// in case of many tiles compared to update count. +// [ e.g. : tileCount >= 10*(newCarveData.size () + m_OldCarveBounds.size ()) ] +// +// Consider letting obstacles push themselves to lists of tiles which they cover. + +PROFILER_INFORMATION (gCrowdManagerCarve, "CrowdManager.CarveNavmesh", kProfilerAI) + + +NavMeshCarving::NavMeshCarving () +{ +} + +NavMeshCarving::~NavMeshCarving () +{ +} + +#if ENABLE_NAVMESH_CARVING + +#include "Runtime/Geometry/AABB.h" +#include "Runtime/Geometry/Intersection.h" +#include "NavMeshTileCarving.h" + +static void CalculateCarveBounds (MinMaxAABB& carveBounds, const NavMeshCarveData& carveData) +{ + AABB localCarveBounds (Vector3f::zero, carveData.size); + AABB worldCarveBounds; + TransformAABBSlow (localCarveBounds, carveData.transform, worldCarveBounds); + carveBounds = worldCarveBounds; +} + +void NavMeshCarving::AddObstacle (NavMeshObstacle& obstacle, int& handle) +{ + Assert (handle == -1); + handle = m_ObstacleInfo.size (); + ObstacleCarveInfo& info = m_ObstacleInfo.push_back (); + info.obstacle = &obstacle; + memset (&info.carveData, 0, sizeof (info.carveData)); +} + +void NavMeshCarving::RemoveObstacle (int& handle) +{ + Assert (handle >= 0 && handle < m_ObstacleInfo.size ()); + const int last = m_ObstacleInfo.size () - 1; + m_OldCarveBounds.push_back (m_ObstacleInfo[handle].carveBounds); + if (handle != last) + { + m_ObstacleInfo[handle] = m_ObstacleInfo[last]; + m_ObstacleInfo[handle].obstacle->SetCarveHandle (handle); + } + handle = -1; + m_ObstacleInfo.pop_back (); +} + +bool NavMeshCarving::Carve () +{ + PROFILER_AUTO (gCrowdManagerCarve, NULL) + + NavMesh* navmesh = GetNavMeshSettings ().GetNavMesh (); + if (navmesh == NULL) + return false; + + // Temporary copy of new data for faster culling + dynamic_array<NavMeshCarveData> newCarveData (m_ObstacleInfo.size (), kMemTempAlloc); + UpdateCarveData (newCarveData); + + if (newCarveData.empty () && m_OldCarveBounds.empty ()) + return false; + + return UpdateTiles (navmesh, newCarveData); +} + +// For registered obstacles - collect info for those that need updating +void NavMeshCarving::UpdateCarveData (dynamic_array<NavMeshCarveData>& newCarveData) +{ + newCarveData.resize_uninitialized (0); + + const size_t obstacleCount = m_ObstacleInfo.size (); + for (size_t i = 0; i < obstacleCount; ++i) + { + if (!m_ObstacleInfo[i].obstacle->NeedsRebuild ()) + continue; + + // Store previous carved data + m_OldCarveBounds.push_back (m_ObstacleInfo[i].carveBounds); + NavMeshCarveData& data = newCarveData.push_back (); + m_ObstacleInfo[i].obstacle->WillRebuildNavmesh (data); + m_ObstacleInfo[i].carveData = data; + CalculateCarveBounds (m_ObstacleInfo[i].carveBounds, data); + } +} + +// Extend the tile bounds by the carving dimensions +// note that the asymmetry in the vertical direction. +static void CalculateExtendedTileBounds (MinMaxAABB& bounds, const dtMeshTile* tile) +{ + const Vector3f tileMin = Vector3f (tile->header->bmin); + const Vector3f tileMax = Vector3f (tile->header->bmax); + const float horizontalMargin = tile->header->walkableRadius; + const float depthMargin = tile->header->walkableRadius; + + bounds.m_Min = Vector3f (tileMin.x - horizontalMargin, tileMin.y, tileMin.z - horizontalMargin); + bounds.m_Max = Vector3f (tileMax.x + horizontalMargin, tileMax.y + depthMargin, tileMax.z + horizontalMargin); +} + +bool NavMeshCarving::UpdateTiles (NavMesh* navmesh, const dynamic_array<NavMeshCarveData>& newCarveData) +{ + dtNavMesh* detourNavMesh = navmesh->GetInternalNavMesh (); + const size_t tileCount = detourNavMesh->tileCount (); + const size_t obstacleCount = m_ObstacleInfo.size (); + + dynamic_array<Vector3f> sizes (obstacleCount, kMemTempAlloc); + dynamic_array<Matrix4x4f> transforms (obstacleCount, kMemTempAlloc); + dynamic_array<MinMaxAABB> aabbs (obstacleCount, kMemTempAlloc); + + int updatedTileCount = 0; + for (size_t i = 0; i < tileCount; ++i) + { + const dtMeshTile* tile = detourNavMesh->getTile (i); + if (!tile || !tile->header) + continue; + + MinMaxAABB tileBounds; + CalculateExtendedTileBounds (tileBounds, tile); + + const TileCarveStatus status = CollectCarveDataAndStatus (transforms, sizes, aabbs, newCarveData, tileBounds); + DebugAssert (transforms.size () == aabbs.size ()); + DebugAssert (transforms.size () == sizes.size ()); + if (status == kIgnore) + continue; + + // Reinitialize tile since we have either 'kRestore' or 'kCarve' at this point + updatedTileCount++; + detourNavMesh->restoreTile (navmesh->GetMeshData (), navmesh->GetMeshDataSize (), i); + + if (status == kCarve) + { + CarveNavMeshTile (tile, detourNavMesh, transforms.size (), transforms.begin (), sizes.begin (), aabbs.begin ()); + } + } + + m_OldCarveBounds.resize_uninitialized (0); + + return updatedTileCount > 0; +} + +// Does any of the bounds in 'arrayOfBounds' overlap with 'bounds' +static bool AnyOverlaps (const dynamic_array<MinMaxAABB>& arrayOfBounds, const MinMaxAABB& bounds) +{ + const size_t count = arrayOfBounds.size (); + for (size_t i = 0; i < count; ++i) + { + if (IntersectAABBAABB (arrayOfBounds[i], bounds)) + return true; + } + return false; +} + +NavMeshCarving::TileCarveStatus NavMeshCarving::CollectCarveDataAndStatus (dynamic_array<Matrix4x4f>& transforms, dynamic_array<Vector3f>& sizes, dynamic_array<MinMaxAABB>& aabbs, const dynamic_array<NavMeshCarveData>& newCarveData, const MinMaxAABB& tileBounds) const +{ + CollectOverlappingCarveData (transforms, sizes, aabbs, tileBounds); + if (!transforms.empty ()) + return kCarve; + + if (AnyOverlaps (m_OldCarveBounds, tileBounds)) + return kRestore; + + return kIgnore; +} + +void NavMeshCarving::CollectOverlappingCarveData (dynamic_array<Matrix4x4f>& transforms, dynamic_array<Vector3f>& sizes, dynamic_array<MinMaxAABB>& aabbs, const MinMaxAABB& bounds) const +{ + aabbs.resize_uninitialized (0); + sizes.resize_uninitialized (0); + transforms.resize_uninitialized (0); + + const size_t count = m_ObstacleInfo.size (); + for (size_t i = 0; i < count; ++i) + { + if (IntersectAABBAABB (m_ObstacleInfo[i].carveBounds, bounds)) + { + aabbs.push_back (m_ObstacleInfo[i].carveBounds); + sizes.push_back (m_ObstacleInfo[i].carveData.size); + transforms.push_back (m_ObstacleInfo[i].carveData.transform); + } + } +} + +#else +bool NavMeshCarving::Carve () {return false;} +void NavMeshCarving::AddObstacle (NavMeshObstacle& obstacle, int& handle) {} +void NavMeshCarving::RemoveObstacle (int& handle) {} + +#endif // ENABLE_NAVMESH_CARVING diff --git a/Runtime/NavMesh/NavMeshCarving.h b/Runtime/NavMesh/NavMeshCarving.h new file mode 100644 index 0000000..0cec244 --- /dev/null +++ b/Runtime/NavMesh/NavMeshCarving.h @@ -0,0 +1,45 @@ +#pragma once + +#include "Runtime/Utilities/dynamic_array.h" +#include "Runtime/NavMesh/NavMeshTypes.h" +#include "Runtime/Geometry/AABB.h" + +class NavMeshObstacle; +class NavMesh; + +class NavMeshCarving +{ + enum TileCarveStatus + { + kIgnore = 0, + kRestore = 1, + kCarve = 2 + }; + + struct ObstacleCarveInfo + { + NavMeshCarveData carveData; + MinMaxAABB carveBounds; + NavMeshObstacle* obstacle; + }; + +public: + NavMeshCarving (); + ~NavMeshCarving (); + + + void AddObstacle (NavMeshObstacle& obstacle, int& handle); + void RemoveObstacle (int& handle); + bool Carve (); + +private: + + void UpdateCarveData (dynamic_array<NavMeshCarveData>& newCarveData); + bool UpdateTiles (NavMesh* navmesh, const dynamic_array<NavMeshCarveData>& newCarveData); + + TileCarveStatus CollectCarveDataAndStatus (dynamic_array<Matrix4x4f>& transforms, dynamic_array<Vector3f>& sizes, dynamic_array<MinMaxAABB>& aabbs, const dynamic_array<NavMeshCarveData>& newCarveData, const MinMaxAABB& tileBounds) const; + void CollectOverlappingCarveData (dynamic_array<Matrix4x4f>& transforms, dynamic_array<Vector3f>& sizes, dynamic_array<MinMaxAABB>& aabbs, const MinMaxAABB& bounds) const; + + dynamic_array<ObstacleCarveInfo> m_ObstacleInfo; + dynamic_array<MinMaxAABB> m_OldCarveBounds; +}; diff --git a/Runtime/NavMesh/NavMeshLayers.cpp b/Runtime/NavMesh/NavMeshLayers.cpp new file mode 100644 index 0000000..bae3674 --- /dev/null +++ b/Runtime/NavMesh/NavMeshLayers.cpp @@ -0,0 +1,156 @@ +#include "UnityPrefix.h" +#include "NavMeshLayers.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Serialize/SerializationMetaFlags.h" +#include "Runtime/BaseClasses/ManagerContext.h" +#include "NavMeshManager.h" + + +const char* NavMeshLayers::s_WarningCostLessThanOne = "Setting a NavMeshLayer cost less than one can give unexpected results."; + +NavMeshLayers::NavMeshLayers (MemLabelId& label, ObjectCreationMode mode) +: Super (label, mode) +{ + +} + +NavMeshLayers::~NavMeshLayers () +{ + +} + +void NavMeshLayers::Reset () +{ + Super::Reset (); + + m_Layers[kNotWalkable].name = "Not Walkable"; + m_Layers[kNotWalkable].cost = 1.0f; + m_Layers[kNotWalkable].editType = NavMeshLayerData::kEditNone; + + m_Layers[kDefaultLayer].name = "Default"; + m_Layers[kDefaultLayer].cost = 1.0f; + m_Layers[kDefaultLayer].editType = NavMeshLayerData::kEditCost; + + m_Layers[kJumpLayer].name = "Jump"; + m_Layers[kJumpLayer].cost = 2.0f; + m_Layers[kJumpLayer].editType = NavMeshLayerData::kEditCost; + + for (int i = kBuiltinLayerCount; i < kLayerCount; ++i) + { + m_Layers[i].cost = 1.0F; + m_Layers[i].editType = NavMeshLayerData::kEditCost | NavMeshLayerData::kEditName; + } +} + + +template<class TransferFunction> +void NavMeshLayers::NavMeshLayerData::Transfer (TransferFunction& transfer) +{ + TransferMetaFlags nameFlag = (editType & kEditName) ? kNoTransferFlags : kNotEditableMask; + TransferMetaFlags costFlag = (editType & kEditCost) ? kNoTransferFlags : kNotEditableMask; + transfer.Transfer (name, "name", nameFlag); + transfer.Transfer (cost, "cost", costFlag); + transfer.Transfer (editType, "editType", kNotEditableMask|kHideInEditorMask); +} + +template<class TransferFunction> +void NavMeshLayers::Transfer (TransferFunction& transfer) +{ + Super::Transfer (transfer); + + for (int i = 0; i < kLayerCount; ++i) + { + char name[64]; + if (i < kBuiltinLayerCount) + sprintf (name, "Built-in Layer %d", i); + else + sprintf (name, "User Layer %d", i - kBuiltinLayerCount); + + transfer.Transfer (m_Layers[i], name); + } +} + +void NavMeshLayers::SetLayerCost (unsigned int index, float cost) +{ + if (index >= kLayerCount) + { + ErrorString ("Index out of bounds"); + return; + } +#if UNITY_EDITOR + if (cost < 1.0f) + { + WarningString(s_WarningCostLessThanOne); + } +#endif + m_Layers[index].cost = cost; + GetNavMeshManager ().UpdateAllNavMeshAgentCosts (index, cost); + + SetDirty (); +} + +float NavMeshLayers::GetLayerCost (unsigned int index) const +{ + if (index >= kLayerCount) + { + ErrorString ("Index out of bounds"); + return 0.0F; + } + return m_Layers[index].cost; +} + +void NavMeshLayers::AwakeFromLoad (AwakeFromLoadMode awakeMode) +{ + Super::AwakeFromLoad (awakeMode); + + // When the user changes the cost in the inspector + if (UNITY_EDITOR && (awakeMode & kDidLoadFromDisk) == 0) + { + for (int i = 0; i < kLayerCount; ++i) + GetNavMeshManager ().UpdateAllNavMeshAgentCosts (i, m_Layers[i].cost); + } +} + +int NavMeshLayers::GetNavMeshLayerFromName (const UnityStr& layerName) const +{ + for (int i = 0; i < kLayerCount; ++i) + { + if (m_Layers[i].name.compare (layerName) == 0) + { + return i; + } + } + return -1; +} + +std::vector<std::string> NavMeshLayers::NavMeshLayerNames () const +{ + std::vector<std::string> layers; + for (int i = 0; i < kLayerCount; ++i) + { + if (m_Layers[i].name.length () != 0) + { + layers.push_back (m_Layers[i].name); + } + } + return layers; +} + +void NavMeshLayers::CheckConsistency () +{ +#if UNITY_EDITOR + for (int i = 0; i < kLayerCount; ++i) + { + if (m_Layers[i].cost < 1.0f) + { + WarningString (s_WarningCostLessThanOne); + return; + } + } +#endif +} + + +IMPLEMENT_CLASS (NavMeshLayers) +IMPLEMENT_OBJECT_SERIALIZE (NavMeshLayers) +GET_MANAGER (NavMeshLayers) diff --git a/Runtime/NavMesh/NavMeshLayers.h b/Runtime/NavMesh/NavMeshLayers.h new file mode 100644 index 0000000..0c4085f --- /dev/null +++ b/Runtime/NavMesh/NavMeshLayers.h @@ -0,0 +1,63 @@ +#ifndef NAVMESH_LAYERS_H +#define NAVMESH_LAYERS_H + +#include "Runtime/BaseClasses/GameManager.h" + + + + +class NavMeshLayers : public GlobalGameManager +{ +public: + struct NavMeshLayerData + { + DECLARE_SERIALIZE (NavMeshLayerData) + enum + { + kEditNone = 0, + kEditName = 1, + kEditCost = 2 + }; + + UnityStr name; + float cost; + int editType; + }; + + enum BuiltinNavMeshLayers + { + kDefaultLayer = 0, + kNotWalkable = 1, + kJumpLayer = 2 + }; + + NavMeshLayers (MemLabelId& label, ObjectCreationMode mode); + // ~NavMeshLayers (); declared-by-macro + + REGISTER_DERIVED_CLASS (NavMeshLayers, GlobalGameManager) + DECLARE_OBJECT_SERIALIZE (NavMeshLayers) + + virtual void Reset (); + virtual void AwakeFromLoad (AwakeFromLoadMode awakeMode); + virtual void CheckConsistency (); + + void SetLayerCost (unsigned int index, float cost); + float GetLayerCost (unsigned int index) const; + int GetNavMeshLayerFromName (const UnityStr& layerName) const; + std::vector<std::string> NavMeshLayerNames () const; + + enum + { + kBuiltinLayerCount = 3, + kLayerCount = 32 + }; + + static const char* s_WarningCostLessThanOne; +private: + + NavMeshLayerData m_Layers[kLayerCount]; +}; + +NavMeshLayers& GetNavMeshLayers (); + +#endif diff --git a/Runtime/NavMesh/NavMeshManager.cpp b/Runtime/NavMesh/NavMeshManager.cpp new file mode 100644 index 0000000..f909996 --- /dev/null +++ b/Runtime/NavMesh/NavMeshManager.cpp @@ -0,0 +1,388 @@ +#include "UnityPrefix.h" +#include "NavMeshManager.h" + +#include "DetourFeatures.h" +#include "DetourCrowd.h" +#include "DetourCrowdTypes.h" +#include "HeightMeshQuery.h" +#include "Runtime/Input/TimeManager.h" +#include "NavMeshAgent.h" +#include "NavMeshObstacle.h" +#include "NavMeshProfiler.h" +#include "OffMeshLink.h" +#include "NavMeshCarving.h" +#include "Runtime/Threads/JobScheduler.h" +#include "Runtime/BaseClasses/IsPlaying.h" +#include "Runtime/Core/Callbacks/PlayerLoopCallbacks.h" +#include "Runtime/Misc/BuildSettings.h" + + +static const int MAX_ITERS_PER_UPDATE = 100; +const int NavMeshManager::kInitialAgentCount = 4; + +PROFILER_INFORMATION (gCrowdManagerUpdate, "CrowdManager.Update", kProfilerAI) +PROFILER_INFORMATION (gNavMeshAgentsUpdateState, "CrowdManager.NavMeshAgentStates", kProfilerAI) +PROFILER_INFORMATION (gNavMeshAgentsUpdateTransform, "CrowdManager.NavMeshAgentTransforms", kProfilerAI) + +NavMeshManager::NavMeshManager () +{ + m_CarvingSystem = NULL; + m_CrowdSystem = NULL; + m_CrowdAgentDebugInfo = NULL; + m_Profiler = UNITY_NEW (CrowdProfiler, kMemNavigation) (); +#if UNITY_EDITOR + m_CrowdAgentDebugInfo = UNITY_NEW (dtCrowdAgentDebugInfo, kMemNavigation); + memset (m_CrowdAgentDebugInfo, 0, sizeof (*m_CrowdAgentDebugInfo)); + m_CrowdAgentDebugInfo->idx = -1; +#endif +} + +NavMeshManager::~NavMeshManager () +{ + if (m_CrowdAgentDebugInfo) + { + UNITY_DELETE (m_CrowdAgentDebugInfo, kMemNavigation); + } + UNITY_DELETE (m_CrowdSystem, kMemNavigation); + UNITY_DELETE (m_Profiler, kMemNavigation); + UNITY_DELETE (m_CarvingSystem, kMemNavigation); +} + +template <typename T> +static inline int RegisterInArray (dynamic_array<T*>& array, T& element) +{ + int handle = array.size (); + array.push_back (&element); + return handle; +} + +template <typename T> +static inline void UnregisterFromArray (dynamic_array<T*>& array, int handle) +{ + Assert (handle >= 0 && handle < array.size ()); + int last = array.size () - 1; + if (handle != last) + { + T* swap = array[last]; + array[handle] = swap; + swap->SetManagerHandle (handle); + } + array.pop_back (); +} + +void NavMeshManager::RegisterAgent (NavMeshAgent& agent, int& handle) +{ + Assert (handle == -1); + handle = RegisterInArray (m_Agents, agent); +} + +void NavMeshManager::UnregisterAgent (int& handle) +{ + UnregisterFromArray (m_Agents, handle); + handle = -1; +} + +void NavMeshManager::RegisterObstacle (NavMeshObstacle& obstacle, int& handle) +{ + Assert (handle == -1); + handle = RegisterInArray (m_Obstacles, obstacle); +} + +void NavMeshManager::UnregisterObstacle (int& handle) +{ + UnregisterFromArray (m_Obstacles, handle); + handle = -1; +} + +void NavMeshManager::RegisterOffMeshLink (OffMeshLink& link, int& handle) +{ + Assert (handle == -1); + handle = RegisterInArray (m_Links, link); +} + +void NavMeshManager::UnregisterOffMeshLink (int& handle) +{ + UnregisterFromArray (m_Links, handle); + handle = -1; +} + +#if ENABLE_NAVMESH_CARVING +void NavMeshManager::UpdateCarving () +{ + if (m_CarvingSystem && m_CarvingSystem->Carve ()) + { + InvalidateDynamicLinks (); + for (size_t i=0;i<m_Agents.size (); ++i) + { + m_Agents[i]->OnNavMeshChanged (); + } + } +} +#else +void NavMeshManager::UpdateCarving () {} +#endif + + +#if DT_DYNAMIC_OFFMESHLINK +PROFILER_INFORMATION (gCrowdManagerLinks, "CrowdManager.DynamicOffMeshLinks", kProfilerAI) + +#include "Runtime/NavMesh/NavMesh.h" +#include "Runtime/NavMesh/NavMeshSettings.h" +void NavMeshManager::InvalidateDynamicLinks () +{ + NavMesh* navmesh = GetNavMeshSettings ().GetNavMesh (); + dtNavMesh* detourNavMesh = navmesh->GetInternalNavMesh (); + detourNavMesh->ClearDynamicOffMeshLinks (); + for (size_t i = 0; i < m_Links.size (); ++i) + { + m_Links[i]->OnNavMeshChanged (); + } +} + +void NavMeshManager::UpdateDynamicLinks () +{ + PROFILER_AUTO (gCrowdManagerLinks, NULL) + if (IsWorldPlaying ()) + { + for (size_t i = 0; i < m_Links.size (); ++i) + m_Links[i]->UpdateMovedPositions (); + } + else + { + for (size_t i = 0; i < m_Links.size (); ++i) + m_Links[i]->UpdatePositions (); + } +} +#else +void NavMeshManager::InvalidateDynamicLinks () {} +void NavMeshManager::UpdateDynamicLinks () {} +#endif + +void NavMeshManager::UpdateCrowdSystem (float deltaTime) +{ + m_CrowdSystem->update (deltaTime, m_Profiler); +} + +void NavMeshManager::Update () +{ + if (GetInternalNavMeshQuery () == NULL) + return; + + UpdateCarving (); + UpdateDynamicLinks (); + + const float deltaTime = GetDeltaTime (); + if (deltaTime == 0.0f) + return; + + PROFILER_BEGIN (gCrowdManagerUpdate, NULL) + + UpdateCrowdSystem (deltaTime); + + PROFILER_BEGIN (gNavMeshAgentsUpdateState, NULL) + for (size_t i = 0; i < m_Agents.size (); ++i) + { + m_Agents[i]->UpdateState (); + } + PROFILER_END + + PROFILER_BEGIN (gNavMeshAgentsUpdateTransform, NULL) + for (size_t i = 0; i < m_Agents.size (); ++i) + { + m_Agents[i]->UpdateTransform (deltaTime); + } + PROFILER_END + + PROFILER_END +} + +// Cleanup references to 'mesh'. +// Skips cleanup if the 'mesh' is not the currently loaded internal navmesh. +// This is needed because when loading scenes we'll temporarily have two instances of NavMesh class. +// (i.e. ctor of new NavMesh object is call before dtor of old NavMesh object). +void NavMeshManager::CleanupMeshDependencies (const dtNavMesh* mesh) +{ + if (mesh == GetInternalNavMesh ()) + { + CleanupMeshDependencies (); + } +} + +// Unconditionally cleanup the navmesh dependencies +void NavMeshManager::CleanupMeshDependencies () +{ + NotifyNavMeshCleanup (); + + if (m_CrowdSystem) + { + m_CrowdSystem->purge (); + UNITY_DELETE (m_CrowdSystem, kMemNavigation); + } +} + +const dtMeshHeader* NavMeshManager::GetNavMeshHeader (const dtNavMesh* navmesh) +{ + if (navmesh == NULL || navmesh->tileCount () == 0) + { + return NULL; + } + return navmesh->getTile (0)->header; +} + +void NavMeshManager::Initialize (const dtNavMesh* navMesh, const HeightMeshQuery* heightMeshQuery) +{ + InitializeCarvingSystem (); + + const dtMeshHeader* header = GetNavMeshHeader (navMesh); + if (!header) + { + CleanupMeshDependencies (); + return; + } + + if (!InitializeCrowdSystem (navMesh, heightMeshQuery, header)) + { + CleanupMeshDependencies (); + return; + } + + InitializeObstacleSamplingQuality (); + + NotifyNavMeshChanged (); +} + +void NavMeshManager::InitializeCarvingSystem () +{ +#if ENABLE_NAVMESH_CARVING + // Carving requires advanced version. Otherwise leave the null pointer for 'm_CarvingSystem'. + if (!m_CarvingSystem && GetBuildSettings ().hasAdvancedVersion) + { + m_CarvingSystem = UNITY_NEW (NavMeshCarving, kMemNavigation); + } +#endif +} + +bool NavMeshManager::InitializeCrowdSystem (const dtNavMesh* navmesh, const HeightMeshQuery* heightMeshQuery, const dtMeshHeader* header) +{ + Assert (navmesh); + + // Lazily create crowd manager + if (m_CrowdSystem == NULL) + { + m_CrowdSystem = UNITY_NEW (dtCrowd, kMemNavigation) (); + if (m_CrowdSystem == NULL) + { + return false; + } + m_CrowdSystem->init (kInitialAgentCount, MAX_ITERS_PER_UPDATE); /////@TODO: WRONG + } + + const float queryRange = 10.0f*header->walkableRadius; ///@TODO: UN-HACK + if (!m_CrowdSystem->setNavMesh (navmesh, queryRange)) + { + return false; + } + + m_CrowdSystem->setHeightMeshQuery (heightMeshQuery); + + return true; +} + +void NavMeshManager::InitializeObstacleSamplingQuality () +{ + // Setup local avoidance params to different qualities. + dtObstacleAvoidanceParams params; + // Use mostly default settings, copy from dtCrowd. + memcpy (¶ms, m_CrowdSystem->getObstacleAvoidanceParams (0), sizeof (dtObstacleAvoidanceParams)); + + // Low (11) + params.adaptiveDivs = 5; + params.adaptiveRings = 2; + params.adaptiveDepth = 1; + m_CrowdSystem->setObstacleAvoidanceParams (kLowQualityObstacleAvoidance, ¶ms); + + // Medium (22) + params.adaptiveDivs = 5; + params.adaptiveRings = 2; + params.adaptiveDepth = 2; + m_CrowdSystem->setObstacleAvoidanceParams (kMedQualityObstacleAvoidance, ¶ms); + + // Good (45) + params.adaptiveDivs = 7; + params.adaptiveRings = 2; + params.adaptiveDepth = 3; + m_CrowdSystem->setObstacleAvoidanceParams (kGoodQualityObstacleAvoidance, ¶ms); + + // High (66) + params.adaptiveDivs = 7; + params.adaptiveRings = 3; + params.adaptiveDepth = 3; + m_CrowdSystem->setObstacleAvoidanceParams (kHighQualityObstacleAvoidance, ¶ms); +} + +void NavMeshManager::NotifyNavMeshChanged () +{ + for (size_t i = 0; i < m_Agents.size (); ++i) + m_Agents[i]->OnNavMeshChanged (); + + for (size_t i = 0; i < m_Obstacles.size (); ++i) + m_Obstacles[i]->OnNavMeshChanged (); + + for (size_t i = 0; i < m_Links.size (); ++i) + m_Links[i]->OnNavMeshChanged (); +} + +void NavMeshManager::NotifyNavMeshCleanup () +{ + for (size_t i = 0; i < m_Agents.size (); ++i) + m_Agents[i]->OnNavMeshCleanup (); + + for (size_t i = 0; i < m_Obstacles.size (); ++i) + m_Obstacles[i]->OnNavMeshCleanup (); + + for (size_t i = 0; i < m_Links.size (); ++i) + m_Links[i]->OnNavMeshCleanup (); +} + +void NavMeshManager::UpdateAllNavMeshAgentCosts (int layerIndex, float layerCost) +{ + if (m_CrowdSystem != NULL) + { + m_CrowdSystem->UpdateFilterCost (layerIndex, layerCost); + } +} + +const dtNavMeshQuery* NavMeshManager::GetInternalNavMeshQuery () const +{ + if (m_CrowdSystem == NULL) + return NULL; + return m_CrowdSystem->getNavMeshQuery (); +} + +const dtNavMesh* NavMeshManager::GetInternalNavMesh () const +{ + if (m_CrowdSystem == NULL || m_CrowdSystem->getNavMeshQuery () == NULL) + return NULL; + return m_CrowdSystem->getNavMeshQuery ()->getAttachedNavMesh (); +} + +static NavMeshManager* gManager = NULL; + +void InitializeNavMeshManager () +{ + Assert (gManager == NULL); + gManager = UNITY_NEW (NavMeshManager, kMemNavigation) (); + + REGISTER_PLAYERLOOP_CALL(NavMeshUpdate, GetNavMeshManager ().Update ()); +} + +void CleanupNavMeshManager () +{ + UNITY_DELETE (gManager, kMemNavigation); + gManager = NULL; +} + +NavMeshManager& GetNavMeshManager () +{ + return *gManager; +} diff --git a/Runtime/NavMesh/NavMeshManager.h b/Runtime/NavMesh/NavMeshManager.h new file mode 100644 index 0000000..e761ef7 --- /dev/null +++ b/Runtime/NavMesh/NavMeshManager.h @@ -0,0 +1,96 @@ +#pragma once + +#include "Runtime/BaseClasses/GameManager.h" + +class CrowdProfiler; +class HeightMeshQuery; +class NavMeshAgent; +class NavMeshCarving; +class NavMeshObstacle; +class OffMeshLink; +class dtCrowd; +class dtNavMesh; +class dtNavMeshQuery; +struct dtCrowdAgentDebugInfo; +struct dtMeshHeader; + + +class NavMeshManager +{ +public: + + NavMeshManager (); + ~NavMeshManager (); + + // NavMeshModule Interface + virtual void Update (); + + void Initialize (const dtNavMesh* navMesh, const HeightMeshQuery* heightMeshQuery); + void CleanupMeshDependencies (const dtNavMesh* mesh); + void CleanupMeshDependencies (); + + inline dtCrowd* GetCrowdSystem (); + inline NavMeshCarving* GetCarvingSystem (); + const dtNavMeshQuery* GetInternalNavMeshQuery () const; + const dtNavMesh* GetInternalNavMesh () const; + + void RegisterAgent (NavMeshAgent& agent, int& handle); + void UnregisterAgent (int& handle); + + void RegisterObstacle (NavMeshObstacle& obstacle, int& handle); + void UnregisterObstacle (int& handle); + + void RegisterOffMeshLink (OffMeshLink& link, int& handle); + void UnregisterOffMeshLink (int& handle); + + void UpdateAllNavMeshAgentCosts (int layerIndex, float layerCost); + +#if UNITY_EDITOR + inline dtCrowdAgentDebugInfo* GetInternalDebugInfo (); +#endif + +private: + const dtMeshHeader* GetNavMeshHeader (const dtNavMesh* navmesh); + bool InitializeCrowdSystem (const dtNavMesh* navmesh, const HeightMeshQuery* heightMeshQuery, const dtMeshHeader* header); + void InitializeObstacleSamplingQuality (); + void InitializeCarvingSystem (); + + void NotifyNavMeshChanged (); + void NotifyNavMeshCleanup (); + void UpdateCrowdSystem (float deltaTime); + void UpdateCarving (); + void UpdateDynamicLinks (); + void InvalidateDynamicLinks (); + + dynamic_array<NavMeshAgent*> m_Agents; + dynamic_array<NavMeshObstacle*> m_Obstacles; + dynamic_array<OffMeshLink*> m_Links; + + NavMeshCarving* m_CarvingSystem; + + dtCrowd* m_CrowdSystem; + dtCrowdAgentDebugInfo* m_CrowdAgentDebugInfo; + CrowdProfiler* m_Profiler; + static const int kInitialAgentCount; +}; + +inline dtCrowd* NavMeshManager::GetCrowdSystem () +{ + return m_CrowdSystem; +} + +inline NavMeshCarving* NavMeshManager::GetCarvingSystem () +{ + return m_CarvingSystem; +} + +#if UNITY_EDITOR +inline dtCrowdAgentDebugInfo* NavMeshManager::GetInternalDebugInfo () +{ + return m_CrowdAgentDebugInfo; +} +#endif // UNITY_EDITOR + +NavMeshManager& GetNavMeshManager (); +void InitializeNavMeshManager (); +void CleanupNavMeshManager (); diff --git a/Runtime/NavMesh/NavMeshModule.jam b/Runtime/NavMesh/NavMeshModule.jam new file mode 100644 index 0000000..d8f27fa --- /dev/null +++ b/Runtime/NavMesh/NavMeshModule.jam @@ -0,0 +1,114 @@ +rule NavMeshModule_ReportCpp +{ + local navMeshSources = + NavMeshModule.jam + + DynamicMeshTests.cpp + DynamicMesh.cpp + DynamicMesh.h + HeightMeshQuery.cpp + HeightMeshQuery.h + HeightmapData.h + NavMesh.cpp + NavMesh.h + NavMeshAgent.cpp + NavMeshAgent.h + NavMeshCarving.cpp + NavMeshCarving.h + NavMeshTileCarving.cpp + NavMeshTileCarving.h + NavMeshTileConversion.cpp + NavMeshTileConversion.h + NavMeshLayers.cpp + NavMeshLayers.h + NavigationModuleRegistration.cpp + NavMeshManager.cpp + NavMeshManager.h + NavMeshObstacle.cpp + NavMeshObstacle.h + NavMeshPath.cpp + NavMeshPath.h + NavMeshProfiler.h + NavMeshSettings.cpp + NavMeshSettings.h + NavMeshTypes.h + OffMeshLink.cpp + OffMeshLink.h + ; + + local detourSources = + Detour/Include/DetourAlloc.h + Detour/Include/DetourAssert.h + Detour/Include/DetourCommon.h + Detour/Include/DetourContext.h + Detour/Include/DetourDynamicLink.h + Detour/Include/DetourFeatures.h + Detour/Include/DetourNavMesh.h + Detour/Include/DetourNavMeshBuilder.h + Detour/Include/DetourNavMeshQuery.h + Detour/Include/DetourNearestPolyQuery.h + Detour/Include/DetourNode.h + Detour/Include/DetourQueryFilter.h + Detour/Include/DetourReference.h + Detour/Include/DetourSwapEndian.h + Detour/Source/DetourAlloc.cpp + Detour/Source/DetourCommon.cpp + Detour/Source/DetourNavMesh.cpp + Detour/Source/DetourNavMeshBuilder.cpp + Detour/Source/DetourNavMeshQuery.cpp + Detour/Source/DetourNearestPolyQuery.cpp + Detour/Source/DetourNode.cpp + Detour/Source/DetourSwapEndian.cpp + DetourCrowd/Include/DetourCrowd.h + DetourCrowd/Include/DetourCrowdUpdate.h + DetourCrowd/Include/DetourCrowdTypes.h + DetourCrowd/Include/DetourLocalBoundary.h + DetourCrowd/Include/DetourObstacleAvoidance.h + DetourCrowd/Include/DetourOccupied.h + DetourCrowd/Include/DetourPathCorridor.h + DetourCrowd/Include/DetourPathQueue.h + DetourCrowd/Include/DetourProximityGrid.h + DetourCrowd/Source/DetourCrowd.cpp + DetourCrowd/Source/DetourCrowdUpdate.cpp + DetourCrowd/Source/DetourLocalBoundary.cpp + DetourCrowd/Source/DetourObstacleAvoidance.cpp + DetourCrowd/Source/DetourOccupied.cpp + DetourCrowd/Source/DetourPathCorridor.cpp + DetourCrowd/Source/DetourPathQueue.cpp + DetourCrowd/Source/DetourProximityGrid.cpp + ; + + local modulesources = + Runtime/NavMesh/$(navMeshSources) + External/Recast/$(detourSources) + ; + + return $(modulesources) ; +} + +rule NavMeshModule_ReportTxt +{ + return + Runtime/NavMesh/ScriptBindings/NavMeshAgentBindings.txt + Runtime/NavMesh/ScriptBindings/NavMeshBindings.txt + Runtime/NavMesh/ScriptBindings/NavMeshPathBindings.txt + Runtime/NavMesh/ScriptBindings/NavMeshObstacleBindings.txt + ; +} + +rule NavMeshModule_ReportIncludes +{ + return + External/Recast/Detour/Include + External/Recast/DetourCrowd/include + ; +} + +rule NavMeshModule_Init +{ + OverrideModule NavMesh : GetModule_Cpp : byOverridingWithMethod : NavMeshModule_ReportCpp ; + OverrideModule NavMesh : GetModule_Txt : byOverridingWithMethod : NavMeshModule_ReportTxt ; + OverrideModule NavMesh : GetModule_Inc : byOverridingWithMethod : NavMeshModule_ReportIncludes ; +} + +#RegisterModule NavMesh ; diff --git a/Runtime/NavMesh/NavMeshObstacle.cpp b/Runtime/NavMesh/NavMeshObstacle.cpp new file mode 100644 index 0000000..6808c85 --- /dev/null +++ b/Runtime/NavMesh/NavMeshObstacle.cpp @@ -0,0 +1,349 @@ +#include "UnityPrefix.h" +#include "NavMeshObstacle.h" +#include "Runtime/Graphics/Transform.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Filters/AABBUtility.h" +#include "DetourCrowd.h" +#include "Runtime/Utilities/ValidateArgs.h" +#include "Runtime/BaseClasses/SupportedMessageOptimization.h" +#include "NavMeshTypes.h" +#include "Runtime/BaseClasses/IsPlaying.h" +#include "NavMeshManager.h" +#include "NavMeshCarving.h" + +NavMeshObstacle::NavMeshObstacle (MemLabelId& label, ObjectCreationMode mode) +: Super (label, mode) +{ + m_Velocity = Vector3f::zero; + m_ManagerHandle = -1; +#if ENABLE_NAVMESH_CARVING + m_CarveHandle = -1; + m_Status = kForceRebuild; +#endif + Reset (); +} + +NavMeshObstacle::~NavMeshObstacle () +{ +} + +template<class TransferFunc> +void NavMeshObstacle::Transfer (TransferFunc& transfer) +{ + Super::Transfer (transfer); + TRANSFER (m_Radius); + TRANSFER (m_Height); +#if ENABLE_NAVMESH_CARVING + TRANSFER (m_MoveThreshold); + TRANSFER (m_Carve); +#endif +} + +void NavMeshObstacle::CheckConsistency () +{ + m_Radius = EnsurePositive (m_Radius); + m_Height = EnsurePositive (m_Height); +#if ENABLE_NAVMESH_CARVING + m_MoveThreshold = max (0.0f, m_MoveThreshold); +#endif +} + +UInt32 NavMeshObstacle::CalculateSupportedMessages () +{ + return kSupportsVelocityChanged; +} + +void NavMeshObstacle::Reset () +{ + Super::Reset (); + + m_Radius = 0.5f; + m_Height = 2.0f; +#if ENABLE_NAVMESH_CARVING + m_MoveThreshold = 0.0f; + m_Carve = false; +#endif +} + +void NavMeshObstacle::SmartReset () +{ + Super::SmartReset (); + AABB aabb; + if (GetGameObjectPtr () && CalculateLocalAABB (GetGameObject (), &aabb)) + { + Vector3f extents = aabb.GetCenter () + aabb.GetExtent (); + + SetRadius (max (extents.x, extents.z)); + SetHeight (2.0F*extents.y); + } + else + { + SetRadius (0.5F); + SetHeight (2.0F); + } +} + +void NavMeshObstacle::AddToManager () +{ + GetNavMeshManager ().RegisterObstacle (*this, m_ManagerHandle); + AddToCrowdSystem (); + +#if ENABLE_NAVMESH_CARVING + AddOrRemoveObstacle (); +#endif +} + +void NavMeshObstacle::RemoveFromManager () +{ + RemoveFromCrowdSystem (); + GetNavMeshManager ().UnregisterObstacle (m_ManagerHandle); + +#if ENABLE_NAVMESH_CARVING + if (m_CarveHandle != -1) + { + if (NavMeshCarving* carving = GetNavMeshManager ().GetCarvingSystem ()) + { + carving->RemoveObstacle (m_CarveHandle); + } + } +#endif +} + +void NavMeshObstacle::RemoveFromCrowdSystem () +{ + if (!InCrowdSystem ()) + return; + GetCrowdSystem ()->RemoveObstacle (m_ObstacleHandle); +} + +void NavMeshObstacle::AwakeFromLoad (AwakeFromLoadMode mode) +{ + Super::AwakeFromLoad (mode); + +#if ENABLE_NAVMESH_CARVING + if (m_ManagerHandle != -1) + AddOrRemoveObstacle (); +#endif + + dtCrowd* crowd = GetCrowdSystem (); + if (crowd == NULL || !InCrowdSystem ()) + return; + + const Vector3f position = GetPosition (); + const Vector3f dimensions = GetScaledDimensions (); + crowd->SetObstaclePosition (m_ObstacleHandle, position.GetPtr ()); + crowd->SetObstacleDimensions (m_ObstacleHandle, dimensions.GetPtr ()); +} + +void NavMeshObstacle::OnNavMeshChanged () +{ + if (!InCrowdSystem ()) + { + AddToCrowdSystem (); + } + +#if ENABLE_NAVMESH_CARVING + m_Status |= kForceRebuild; +#endif +} + +void NavMeshObstacle::OnNavMeshCleanup () +{ + RemoveFromCrowdSystem (); +} + +void NavMeshObstacle::AddToCrowdSystem () +{ + if (!IsWorldPlaying ()) + return; + + if (!GetNavMeshManager ().GetInternalNavMeshQuery ()) + return; + + Assert (!InCrowdSystem ()); + dtCrowd* crowd = GetCrowdSystem (); + if (crowd == NULL) + return; + + if (!crowd->AddObstacle (m_ObstacleHandle)) + return; + + const Vector3f position = GetPosition (); + const Vector3f dimensions = GetScaledDimensions (); + crowd->SetObstaclePosition (m_ObstacleHandle, position.GetPtr ()); + crowd->SetObstacleDimensions (m_ObstacleHandle, dimensions.GetPtr ()); +} + +void NavMeshObstacle::InitializeClass () +{ + REGISTER_MESSAGE (NavMeshObstacle, kTransformChanged, OnTransformChanged, int); + REGISTER_MESSAGE_PTR (NavMeshObstacle, kDidVelocityChange, OnVelocityChanged, Vector3f); +} + +void NavMeshObstacle::OnTransformChanged (int mask) +{ +#if ENABLE_NAVMESH_CARVING + m_Status |= kHasMoved; + if (mask & Transform::kRotationChanged) + { + m_Status = kForceRebuild; + } +#endif + if (!InCrowdSystem ()) + return; + + if (mask & Transform::kPositionChanged) + { + const Vector3f position = GetPosition (); + GetCrowdSystem ()->SetObstaclePosition (m_ObstacleHandle, position.GetPtr ()); + } + + if (mask & Transform::kScaleChanged) + { + const Vector3f dimensions = GetScaledDimensions (); + GetCrowdSystem ()->SetObstacleDimensions (m_ObstacleHandle, dimensions.GetPtr ()); + } +} + +#if ENABLE_NAVMESH_CARVING + +void NavMeshObstacle::AddOrRemoveObstacle () +{ + NavMeshCarving* carving = GetNavMeshManager ().GetCarvingSystem (); + if (!carving) + { + return; + } + + if (m_Carve && m_CarveHandle == -1) + { + carving->AddObstacle (*this, m_CarveHandle); + RemoveFromCrowdSystem (); + } + else if (!m_Carve && m_CarveHandle != -1) + { + carving->RemoveObstacle (m_CarveHandle); + AddToCrowdSystem (); + } + + m_Status |= kForceRebuild; +} + +void NavMeshObstacle::WillRebuildNavmesh (NavMeshCarveData& carveData) +{ + const Vector3f position = GetPosition (); + CalculateTransformAndSize (carveData.transform, carveData.size); + + m_LastCarvedPosition = position; + m_Status = kClean; +} + +bool NavMeshObstacle::NeedsRebuild () const +{ + if (m_Status == kClean) + return false; + + if (m_Status & kForceRebuild) + return true; + + if (m_Status & kHasMoved) + { + const Vector3f position = GetComponent (Transform).GetPosition (); + const float sqrDistance = SqrMagnitude (m_LastCarvedPosition - position); + if (sqrDistance > m_MoveThreshold * m_MoveThreshold) + return true; + } + + return false; +} + +void NavMeshObstacle::SetCarving (bool carve) +{ + if (m_Carve == carve) + return; + + m_Carve = carve; + AddOrRemoveObstacle (); + SetDirty (); +} + +void NavMeshObstacle::SetMoveThreshold (float moveThreshold) +{ + ABORT_INVALID_FLOAT (moveThreshold, moveThreshold, navmeshobstacle); + m_MoveThreshold = moveThreshold; + SetDirty (); +} + +#endif + +void NavMeshObstacle::OnVelocityChanged (Vector3f* value) +{ + SetVelocity (*value); +} + +void NavMeshObstacle::SetVelocity (const Vector3f& value) +{ + ABORT_INVALID_VECTOR3 (value, velocity, navmeshobstacle); + m_Velocity = value; + if (InCrowdSystem ()) + { + GetCrowdSystem ()->SetObstacleVelocity (m_ObstacleHandle, m_Velocity.GetPtr ()); + } +} + +void NavMeshObstacle::SetRadius (float value) +{ + ABORT_INVALID_FLOAT (value, radius, navmeshobstacle); + m_Radius = EnsurePositive (value); + SetDirty (); + const Vector3f dimensions = GetScaledDimensions (); + if (InCrowdSystem ()) + { + GetCrowdSystem ()->SetObstacleDimensions (m_ObstacleHandle, dimensions.GetPtr ()); + } +} + +void NavMeshObstacle::SetHeight (float value) +{ + ABORT_INVALID_FLOAT (value, height, navmeshobstacle); + m_Height = EnsurePositive (value); + SetDirty (); + const Vector3f dimensions = GetScaledDimensions (); + if (InCrowdSystem ()) + { + GetCrowdSystem ()->SetObstacleDimensions (m_ObstacleHandle, dimensions.GetPtr ()); + } +} + +Vector3f NavMeshObstacle::GetScaledDimensions () const +{ + Vector3f absScale = Abs (GetComponent (Transform).GetWorldScaleLossy ()); + float scaledRadius = m_Radius * max (absScale.x, absScale.z); + float scaledHeight = m_Height * absScale.y; + return Vector3f (scaledRadius, scaledHeight, scaledRadius); +} + +void NavMeshObstacle::CalculateTransformAndSize (Matrix4x4f& trans, Vector3f& size) +{ + // TODO cache result on obstacle. + const Transform& transform = GetComponent (Transform); + trans = transform.GetLocalToWorldMatrix (); + + AABB aabb; + if (CalculateLocalAABB (GetGameObject (), &aabb)) + { + size = aabb.GetExtent (); + } + else + { + size = Vector3f (m_Radius, m_Height, m_Radius); + } +} + +dtCrowd* NavMeshObstacle::GetCrowdSystem () +{ + return GetNavMeshManager ().GetCrowdSystem (); +} + +IMPLEMENT_CLASS_HAS_INIT (NavMeshObstacle) +IMPLEMENT_OBJECT_SERIALIZE (NavMeshObstacle) diff --git a/Runtime/NavMesh/NavMeshObstacle.h b/Runtime/NavMesh/NavMeshObstacle.h new file mode 100644 index 0000000..17c6e45 --- /dev/null +++ b/Runtime/NavMesh/NavMeshObstacle.h @@ -0,0 +1,155 @@ +#ifndef RUNTIME_NAVMESHOBSTACLE +#define RUNTIME_NAVMESHOBSTACLE + +#include "Runtime/GameCode/Behaviour.h" +#include "Runtime/Graphics/Transform.h" +#include "Runtime/Math/Vector3.h" +#include "NavMeshManager.h" +#include "DetourFeatures.h" +#include "DetourReference.h" + +struct NavMeshCarveData; +class dtCrowd; +class dtNavMeshQuery; +class Matrix4x4f; + +class NavMeshObstacle : public Behaviour +{ +public: + REGISTER_DERIVED_CLASS (NavMeshObstacle, Behaviour) + DECLARE_OBJECT_SERIALIZE (NavMeshObstacle) + + NavMeshObstacle (MemLabelId& label, ObjectCreationMode mode); + // ~NavMeshObstacle (); declared by a macro + + virtual void AwakeFromLoad (AwakeFromLoadMode mode); + static void InitializeClass (); + static void CleanupClass () { } + + inline bool InCrowdSystem () const; + inline void SetManagerHandle (int handle); + + void OnNavMeshChanged (); + void OnNavMeshCleanup (); + +#if ENABLE_NAVMESH_CARVING + inline void SetCarveHandle (int handle); + + void WillRebuildNavmesh (NavMeshCarveData& carveData); + bool NeedsRebuild () const; + + inline bool GetCarving () const; + void SetCarving (bool carve); + + inline float GetMoveThreshold () const; + void SetMoveThreshold (float moveThreshold); +#endif + + Vector3f GetScaledDimensions () const; + inline Vector3f GetPosition () const; + + inline Vector3f GetVelocity () const; + void SetVelocity (const Vector3f& value); + + inline float GetRadius () const; + void SetRadius (float value); + + inline float GetHeight () const; + void SetHeight (float value); + + +protected: + virtual void Reset (); + virtual void SmartReset (); + virtual void AddToManager (); + virtual void RemoveFromManager (); + virtual void CheckConsistency (); + virtual UInt32 CalculateSupportedMessages (); + + void OnVelocityChanged (Vector3f* value); + void OnTransformChanged (int mask); + +private: + void AddToCrowdSystem (); + void RemoveFromCrowdSystem (); + void CalculateTransformAndSize (Matrix4x4f& trans, Vector3f& size); + + static inline float EnsurePositive (float value); + static dtCrowd* GetCrowdSystem (); + + int m_ManagerHandle; + dtCrowdHandle m_ObstacleHandle; + float m_Radius; + float m_Height; + Vector3f m_Velocity; + +#if ENABLE_NAVMESH_CARVING + void AddOrRemoveObstacle (); + + enum + { + kClean = 0, + kHasMoved = 1 << 0, + kForceRebuild = 1 << 1 + }; + float m_MoveThreshold; + Vector3f m_LastCarvedPosition; + UInt32 m_Status; + int m_CarveHandle; + bool m_Carve; +#endif +}; + +inline bool NavMeshObstacle::InCrowdSystem () const +{ + return m_ObstacleHandle.IsValid (); +} + +inline void NavMeshObstacle::SetManagerHandle (int handle) +{ + m_ManagerHandle = handle; +} + +inline float NavMeshObstacle::EnsurePositive (float value) +{ + return std::max (0.00001F, value); +} + +inline Vector3f NavMeshObstacle::GetPosition () const +{ + return GetComponent (Transform).GetPosition (); +} + +inline Vector3f NavMeshObstacle::GetVelocity () const +{ + return m_Velocity; +} + +inline float NavMeshObstacle::GetRadius () const +{ + return m_Radius; +} + +inline float NavMeshObstacle::GetHeight () const +{ + return m_Height; +} + +#if ENABLE_NAVMESH_CARVING +inline void NavMeshObstacle::SetCarveHandle (int handle) +{ + m_CarveHandle = handle; +} + +inline bool NavMeshObstacle::GetCarving () const +{ + return m_Carve; +} + +inline float NavMeshObstacle::GetMoveThreshold () const +{ + return m_MoveThreshold; +} +#endif // ENABLE_NAVMESH_CARVING + +#endif diff --git a/Runtime/NavMesh/NavMeshPath.cpp b/Runtime/NavMesh/NavMeshPath.cpp new file mode 100644 index 0000000..68f51c9 --- /dev/null +++ b/Runtime/NavMesh/NavMeshPath.cpp @@ -0,0 +1,19 @@ +#include "UnityPrefix.h" +#include "NavMeshPath.h" +#include "NavMeshSettings.h" +#include "OffMeshLink.h" +#include "NavMesh.h" + + +NavMeshPath::NavMeshPath () +: m_polygonCount (0) +, m_status (kPathInvalid) +, m_timeStamp (0) +{ +} + +NavMeshPath::~NavMeshPath () +{ +} + + diff --git a/Runtime/NavMesh/NavMeshPath.h b/Runtime/NavMesh/NavMeshPath.h new file mode 100644 index 0000000..e0be375 --- /dev/null +++ b/Runtime/NavMesh/NavMeshPath.h @@ -0,0 +1,90 @@ +#pragma once +#ifndef RUNTIME_NAVMESH_PATH +#define RUNTIME_NAVMESH_PATH + +#include "Runtime/Math/Vector3.h" +#include "NavMeshTypes.h" + +struct OffMeshLinkData; + +class NavMeshPath +{ +public: + enum + { + kMaxPathPolygons = 256 + }; + + NavMeshPath (); + ~NavMeshPath (); + + inline Vector3f GetSourcePosition () const; + inline void SetSourcePosition (const Vector3f& sourcePosition); + inline Vector3f GetTargetPosition () const; + inline void SetTargetPosition (const Vector3f& targetPosition); + inline int GetPolygonCount () const; + inline void SetPolygonCount (int polygonCount); + + inline unsigned int* GetPolygonPath (); + inline const unsigned int* GetPolygonPath () const; + + inline NavMeshPathStatus GetStatus () const; + inline void SetStatus (NavMeshPathStatus status); + inline void SetTimeStamp (unsigned int timeStamp); + +private: + + unsigned int m_timeStamp; + NavMeshPathStatus m_status; + unsigned int m_polygons[kMaxPathPolygons]; + int m_polygonCount; + Vector3f m_sourcePosition; + Vector3f m_targetPosition; +}; + +inline Vector3f NavMeshPath::GetSourcePosition () const +{ + return m_sourcePosition; +} +inline void NavMeshPath::SetSourcePosition (const Vector3f& sourcePosition) +{ + m_sourcePosition = sourcePosition; +} +inline Vector3f NavMeshPath::GetTargetPosition () const +{ + return m_targetPosition; +} +inline void NavMeshPath::SetTargetPosition (const Vector3f& targetPosition) +{ + m_targetPosition = targetPosition; +} +inline int NavMeshPath::GetPolygonCount () const +{ + return m_polygonCount; +} +inline void NavMeshPath::SetPolygonCount (int polygonCount) +{ + m_polygonCount = polygonCount; +} +inline unsigned int* NavMeshPath::GetPolygonPath () +{ + return m_polygons; +} +inline const unsigned int* NavMeshPath::GetPolygonPath () const +{ + return m_polygons; +} +inline NavMeshPathStatus NavMeshPath::GetStatus () const +{ + return m_status; +} +inline void NavMeshPath::SetStatus (NavMeshPathStatus status) +{ + m_status = status; +} +inline void NavMeshPath::SetTimeStamp (unsigned int timeStamp) +{ + m_timeStamp = timeStamp; +} + +#endif diff --git a/Runtime/NavMesh/NavMeshProfiler.h b/Runtime/NavMesh/NavMeshProfiler.h new file mode 100644 index 0000000..122b224 --- /dev/null +++ b/Runtime/NavMesh/NavMeshProfiler.h @@ -0,0 +1,71 @@ +#ifndef NAVMESHPROFILER_H +#define NAVMESHPROFILER_H + +#include "Runtime/Profiler/Profiler.h" +#include "DetourContext.h" + +PROFILER_INFORMATION (gCrowdManagerPathFinding, "CrowdManager.PathFinding", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerPathFollowing, "CrowdManager.PathFollowing", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerPathFollowingLate, "CrowdManager.PathFollowing.Late", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerAvoidance, "CrowdManager.Avoidance", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerAvoidanceSampling, "CrowdManager.Avoidance.Sampling", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerProximity, "CrowdManager.Proximity", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerProximityInsert, "CrowdManager.Proximity.Insert", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerProximityCollect, "CrowdManager.Proximity.Collect", kProfilerAI) +PROFILER_INFORMATION (gCrowdManagerCollision, "CrowdManager.Collision", kProfilerAI) + +class CrowdProfiler : public dtContext +{ +public: + CrowdProfiler () + : dtContext (true) + { + } + virtual ~CrowdProfiler () + { + } + +protected: + virtual void doStartTimer (const dtTimerLabel label) + { + switch (label) + { + case DT_TIMER_UPDATE_PATHFINDING: + PROFILER_BEGIN (gCrowdManagerPathFinding, NULL); + break; + case DT_TIMER_UPDATE_PATHFOLLOWING: + PROFILER_BEGIN (gCrowdManagerPathFollowing, NULL); + break; + case DT_TIMER_UPDATE_PATHFOLLOWING_LATE: + PROFILER_BEGIN (gCrowdManagerPathFollowingLate, NULL); + break; + case DT_TIMER_UPDATE_AVOIDANCE: + PROFILER_BEGIN (gCrowdManagerAvoidance, NULL); + break; + case DT_TIMER_UPDATE_AVOIDANCE_SAMPLING: + PROFILER_BEGIN (gCrowdManagerAvoidanceSampling, NULL); + break; + case DT_TIMER_UPDATE_PROXIMITY: + PROFILER_BEGIN (gCrowdManagerProximity, NULL); + break; + case DT_TIMER_UPDATE_PROXIMITY_INSERT: + PROFILER_BEGIN (gCrowdManagerProximityInsert, NULL); + break; + case DT_TIMER_UPDATE_PROXIMITY_COLLECT: + PROFILER_BEGIN (gCrowdManagerProximityCollect, NULL); + break; + case DT_TIMER_UPDATE_COLLISION: + PROFILER_BEGIN (gCrowdManagerCollision, NULL); + break; + default: + break; + } + } + + virtual void doStopTimer (const dtTimerLabel /*label*/) + { + PROFILER_END + } +}; + +#endif diff --git a/Runtime/NavMesh/NavMeshSettings.cpp b/Runtime/NavMesh/NavMeshSettings.cpp new file mode 100644 index 0000000..49bdd02 --- /dev/null +++ b/Runtime/NavMesh/NavMeshSettings.cpp @@ -0,0 +1,103 @@ +#include "UnityPrefix.h" +#include "NavMeshSettings.h" +#include "NavMeshManager.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/BaseClasses/ManagerContext.h" +#include "Runtime/BaseClasses/IsPlaying.h" +#include "OffMeshLink.h" +#include "NavMeshLayers.h" +#include "NavMesh.h" +#include "HeightmapData.h" +#include "DetourNavMesh.h" + +void NavMeshSettings::InitializeClass () +{ + InitializeNavMeshManager (); +} + +void NavMeshSettings::CleanupClass () +{ + CleanupNavMeshManager (); +} + + +NavMeshSettings::NavMeshSettings (MemLabelId& label, ObjectCreationMode mode) + : Super (label, mode) +{ +} + +NavMeshSettings::~NavMeshSettings () +{ + GetNavMeshManager ().CleanupMeshDependencies (); +} + +void NavMeshSettings::Reset () +{ + Super::Reset (); + + #if UNITY_EDITOR + m_BuildSettings = NavMeshBuildSettings (); + #endif +} + +void NavMeshSettings::AwakeFromLoad (AwakeFromLoadMode mode) +{ + Super::AwakeFromLoad (mode); + + // Initialize NavMesh + const dtNavMesh* internalNavMesh = NULL; + const HeightMeshQuery* heightMeshQuery = NULL; + if (m_NavMesh) + { + // Calling m_NavMesh->Create () here to ensure state of navmesh is restored. + // Were are already copying the data so memory usage is not affected. + m_NavMesh->Create (); + internalNavMesh = m_NavMesh->GetInternalNavMesh (); + heightMeshQuery = m_NavMesh->GetHeightMeshQuery (); + } + else + { + GetNavMeshManager ().CleanupMeshDependencies (); + } + GetNavMeshManager ().Initialize (internalNavMesh, heightMeshQuery); +} + +template<class T> +void NavMeshSettings::Transfer (T& transfer) +{ + Super::Transfer (transfer); + + TRANSFER_EDITOR_ONLY (m_BuildSettings); + TRANSFER (m_NavMesh); +} + +bool NavMeshSettings::SetOffMeshPolyInstanceID (dtPolyRef ref, int instanceID) +{ + if (dtNavMesh* navmesh = GetInternalNavMesh ()) + return navmesh->setOffMeshPolyInstanceID (ref, instanceID) == DT_SUCCESS; + return false; +} + +void NavMeshSettings::SetOffMeshPolyCostOverride (dtPolyRef ref, float costOverride) +{ + if (dtNavMesh* navmesh = GetInternalNavMesh ()) + navmesh->setOffMeshPolyCostOverride (ref, costOverride); +} + +void NavMeshSettings::SetOffMeshPolyAccess (dtPolyRef ref, bool access) +{ + if (dtNavMesh* navmesh = GetInternalNavMesh ()) + navmesh->setOffMeshPolyAccess (ref, access); +} + +dtNavMesh* NavMeshSettings::GetInternalNavMesh () +{ + NavMesh* navmesh = GetNavMesh (); + if (navmesh == NULL) + return NULL; + return navmesh->GetInternalNavMesh (); +} + +IMPLEMENT_OBJECT_SERIALIZE (NavMeshSettings) +IMPLEMENT_CLASS_HAS_INIT (NavMeshSettings) +GET_MANAGER (NavMeshSettings) diff --git a/Runtime/NavMesh/NavMeshSettings.h b/Runtime/NavMesh/NavMeshSettings.h new file mode 100644 index 0000000..6c1b6ca --- /dev/null +++ b/Runtime/NavMesh/NavMeshSettings.h @@ -0,0 +1,69 @@ +#pragma once + +#include "Runtime/BaseClasses/GameManager.h" +#include "Runtime/Utilities/dynamic_array.h" +#include "Runtime/NavMesh/NavMeshTypes.h" +#if UNITY_EDITOR +#include "Editor/Src/NavMesh/NavMeshBuildSettings.h" +#endif + +class NavMesh; +class dtNavMesh; + +class NavMeshSettings : public LevelGameManager +{ +public: + + REGISTER_DERIVED_CLASS (NavMeshSettings, LevelGameManager); + DECLARE_OBJECT_SERIALIZE (NavMeshSettings); + + NavMeshSettings (MemLabelId& label, ObjectCreationMode mode); + + virtual void AwakeFromLoad (AwakeFromLoadMode mode); + virtual void Reset (); + + inline void SetNavMesh (NavMesh* navMesh); + inline NavMesh* GetNavMesh (); + + bool SetOffMeshPolyInstanceID (dtPolyRef ref, int instanceID); + void SetOffMeshPolyCostOverride (dtPolyRef ref, float costOverride); + void SetOffMeshPolyAccess (dtPolyRef ref, bool access); + + + #if UNITY_EDITOR + inline NavMeshBuildSettings& GetNavMeshBuildSettings (); + #endif + + static void InitializeClass (); + static void CleanupClass (); + + dtNavMesh* GetInternalNavMesh (); +private: + +#if UNITY_EDITOR + NavMeshBuildSettings m_BuildSettings; +#endif + + PPtr<NavMesh> m_NavMesh; +}; + +inline void NavMeshSettings::SetNavMesh (NavMesh* navMesh) +{ + m_NavMesh = navMesh; + SetDirty (); +} + +inline NavMesh* NavMeshSettings::GetNavMesh () +{ + return m_NavMesh; +} + +#if UNITY_EDITOR +inline NavMeshBuildSettings& NavMeshSettings::GetNavMeshBuildSettings () +{ + return m_BuildSettings; +} +#endif + +NavMeshSettings& GetNavMeshSettings (); + diff --git a/Runtime/NavMesh/NavMeshTileCarving.cpp b/Runtime/NavMesh/NavMeshTileCarving.cpp new file mode 100644 index 0000000..24565aa --- /dev/null +++ b/Runtime/NavMesh/NavMeshTileCarving.cpp @@ -0,0 +1,176 @@ +#include "UnityPrefix.h" +#include "NavMeshTileCarving.h" +#include "NavMeshTileConversion.h" +#include "DynamicMesh.h" +#include "DetourNavMesh.h" +#include "Runtime/Math/Matrix4x4.h" +#include "Runtime/Geometry/AABB.h" +#include "DetourCommon.h" +#include "DetourAlloc.h" + +// TODO: +// Sort carving objects spatially - so carving order does not depend on carve index + +static inline Vector3f TileMidpoint (const dtMeshHeader* tileHeader); +static bool CalculateHull (DynamicMesh::Hull& carveHull, const Matrix4x4f& transform, const Vector3f& size, const MinMaxAABB& aabb, const Vector3f& tileOffset, const float carveWidth, const float carveDepth); +static Vector3f CalculateCarveOffsetScale (const Vector3f& axis); + +// Replaces a single tile in the detour navmesh with a carved tile. +void CarveNavMeshTile (const dtMeshTile* tile, dtNavMesh* navmesh, size_t count, const Matrix4x4f* transforms, const Vector3f* sizes, const MinMaxAABB* aabbs) +{ + if (count == 0 || tile == NULL || tile->header == NULL) + { + return; + } + + const Vector3f tileOffset = TileMidpoint (tile->header); + const float carveWidth = tile->header->walkableRadius; + const float carveDepth = tile->header->walkableHeight; + + DynamicMesh::HullContainer carveHulls; + for (size_t i = 0; i < count; ++i) + { + DynamicMesh::Hull carveHull; + if (CalculateHull (carveHull, transforms[i], sizes[i], aabbs[i], tileOffset, carveWidth, carveDepth)) + { + carveHulls.push_back (carveHull); + } + } + + DynamicMesh dynamicMesh; + if (!TileToDynamicMesh (tile, dynamicMesh, tileOffset)) + { + return; + } + if (!dynamicMesh.ClipPolys (carveHulls)) + { + return; + } + + dynamicMesh.FindNeighbors (); + + int newTileSize = 0; + unsigned char* newTile = DynamicMeshToTile (&newTileSize, dynamicMesh, tile, tileOffset); + + dtPolyRef tileRef = navmesh->getTileRef (tile); + navmesh->removeTile (tileRef, 0, 0); + dtStatus status = navmesh->addTile (newTile, newTileSize, DT_TILE_FREE_DATA, tileRef, 0); + if (dtStatusFailed (status)) + { + dtFree (newTile); + } +} + +static inline Vector3f TileMidpoint (const dtMeshHeader* tileHeader) +{ + if (!tileHeader) + { + return Vector3f::zero; + } + return 0.5f * (Vector3f (tileHeader->bmin) + Vector3f (tileHeader->bmax)); +} + +static inline bool AreColinear (const Vector3f& v, const Vector3f& u, const float cosAngleAccept) +{ + DebugAssert (IsNormalized (v)); + DebugAssert (IsNormalized (u)); + return Abs (Dot (v, u)) > cosAngleAccept; +} + +// Compute the set of planes defining an extruded bounding box. +// Bounding box is represented by transform and size. +// Extrusion is based on 'carveWidth' horizontally and 'carveDepth' vertically down. +// Everyting is translated relative to 'tileOffset'. +static bool CalculateHull (DynamicMesh::Hull& carveHull, const Matrix4x4f& transform, const Vector3f& size, const MinMaxAABB& aabb, const Vector3f& tileOffset, const float carveWidth, const float carveDepth) +{ + carveHull.resize_uninitialized (12); + + const float cosAngleConsiderAxisAligned = Cos (Deg2Rad (10.0f)); // Consider colinear if within 10 degrees + bool isAlmostAxisAlignedX = false; + bool isAlmostAxisAlignedY = false; + bool isAlmostAxisAlignedZ = false; + + // First add the six planes from the OBB + const Vector3f carveLocalOffset = Vector3f (carveWidth, carveDepth, carveWidth); + const Vector3f position = transform.GetPosition () - tileOffset; + Vector3f axis, offset, planeOffset; + + axis = transform.GetAxisX (); + if (CompareApproximately (axis, Vector3f::zero, 0.0001f)) + { + return false; + } + offset = size.x * axis; + axis = Normalize (axis); + planeOffset = Scale (CalculateCarveOffsetScale (-axis), carveLocalOffset) - offset; + carveHull[0].SetNormalAndPosition (-axis, position + planeOffset); + planeOffset = Scale (CalculateCarveOffsetScale (axis), carveLocalOffset) + offset; + carveHull[1].SetNormalAndPosition (axis, position + planeOffset); + isAlmostAxisAlignedX = isAlmostAxisAlignedX || AreColinear (axis, Vector3f::xAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedY = isAlmostAxisAlignedY || AreColinear (axis, Vector3f::yAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedZ = isAlmostAxisAlignedZ || AreColinear (axis, Vector3f::zAxis, cosAngleConsiderAxisAligned); + + axis = transform.GetAxisY (); + if (CompareApproximately (axis, Vector3f::zero, 0.0001f)) + { + return false; + } + offset = size.y * axis; + axis = Normalize (axis); + planeOffset = Scale (CalculateCarveOffsetScale (-axis), carveLocalOffset) - offset; + carveHull[2].SetNormalAndPosition (-axis, position + planeOffset); + planeOffset = Scale (CalculateCarveOffsetScale (axis), carveLocalOffset) + offset; + carveHull[3].SetNormalAndPosition (axis, position + planeOffset); + isAlmostAxisAlignedX = isAlmostAxisAlignedX || AreColinear (axis, Vector3f::xAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedY = isAlmostAxisAlignedY || AreColinear (axis, Vector3f::yAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedZ = isAlmostAxisAlignedZ || AreColinear (axis, Vector3f::zAxis, cosAngleConsiderAxisAligned); + + axis = transform.GetAxisZ (); + if (CompareApproximately (axis, Vector3f::zero, 0.0001f)) + { + return false; + } + offset = size.z * axis; + axis = Normalize (axis); + planeOffset = Scale (CalculateCarveOffsetScale (-axis), carveLocalOffset) - offset; + carveHull[4].SetNormalAndPosition (-axis, position + planeOffset); + planeOffset = Scale (CalculateCarveOffsetScale (axis), carveLocalOffset) + offset; + carveHull[5].SetNormalAndPosition (axis, position + planeOffset); + isAlmostAxisAlignedX = isAlmostAxisAlignedX || AreColinear (axis, Vector3f::xAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedY = isAlmostAxisAlignedY || AreColinear (axis, Vector3f::yAxis, cosAngleConsiderAxisAligned); + isAlmostAxisAlignedZ = isAlmostAxisAlignedZ || AreColinear (axis, Vector3f::zAxis, cosAngleConsiderAxisAligned); + + int planeCount = 6; + + // The add the six planes from the containing AABB + const Vector3f min = aabb.m_Min - tileOffset; + const Vector3f max = aabb.m_Max - tileOffset; + if (!isAlmostAxisAlignedX) + { + carveHull[planeCount++].SetNormalAndPosition (-Vector3f::xAxis, min - carveWidth*Vector3f::xAxis); + carveHull[planeCount++].SetNormalAndPosition (Vector3f::xAxis, max + carveWidth*Vector3f::xAxis); + } + if (!isAlmostAxisAlignedY) + { + carveHull[planeCount++].SetNormalAndPosition (-Vector3f::yAxis, min - carveDepth*Vector3f::yAxis); + carveHull[planeCount++].SetNormalAndPosition (Vector3f::yAxis, max); + } + if (!isAlmostAxisAlignedZ) + { + carveHull[planeCount++].SetNormalAndPosition (-Vector3f::zAxis, min - carveWidth*Vector3f::zAxis); + carveHull[planeCount++].SetNormalAndPosition (Vector3f::zAxis, max + carveWidth*Vector3f::zAxis); + } + + carveHull.resize_uninitialized (planeCount); + return true; +} + +// Returns the general plane offset direction given the plane axis +static Vector3f CalculateCarveOffsetScale (const Vector3f& axis) +{ + Vector3f res; + res.x = Sign (axis.x); + res.y = std::min (0.0f, Sign (axis.y)); + res.z = Sign (axis.z); + return res; +} diff --git a/Runtime/NavMesh/NavMeshTileCarving.h b/Runtime/NavMesh/NavMeshTileCarving.h new file mode 100644 index 0000000..6ef85a2 --- /dev/null +++ b/Runtime/NavMesh/NavMeshTileCarving.h @@ -0,0 +1,12 @@ +#ifndef _NAVMESHTILECARVING_H_INCLUDED_ +#define _NAVMESHTILECARVING_H_INCLUDED_ + +struct dtMeshTile; +class dtNavMesh; +class Matrix4x4f; +class Vector3f; +class MinMaxAABB; + +void CarveNavMeshTile (const dtMeshTile* tile, dtNavMesh* detourNavMesh, size_t count, const Matrix4x4f* transforms, const Vector3f* sizes, const MinMaxAABB* aabbs); + +#endif diff --git a/Runtime/NavMesh/NavMeshTileConversion.cpp b/Runtime/NavMesh/NavMeshTileConversion.cpp new file mode 100644 index 0000000..5ab6508 --- /dev/null +++ b/Runtime/NavMesh/NavMeshTileConversion.cpp @@ -0,0 +1,408 @@ +#include "UnityPrefix.h" +#include "NavMeshTileConversion.h" +#include "DynamicMesh.h" +#include "DetourNavMesh.h" +#include "DetourCommon.h" +#include "DetourAlloc.h" + +// TODO: +// Create BVH for carved tile +// Preserve detail mesh for the carved polygons. + +const float MAGIC_EDGE_DISTANCE = 1e-2f; // Same as used in detour navmesh builder. + +static void RequirementsForDetailMeshUsingHeightMesh (int* detailVertCount, int* detailTriCount, const DynamicMesh& mesh, const dtMeshTile* sourceTile); +static void RequirementsForDetailMeshMixed (int* detailVertCount, int* detailTriCount, const DynamicMesh& mesh, const dtMeshTile* sourceTile); +static void WritePortalFlags (const float* verts, dtPoly* polys, const int polyCount, const dtMeshHeader* sourceHeader); +static void WriteDetailMeshUsingHeightMesh (dtPolyDetail* detail, float* dverts, dtPolyDetailIndex* dtris + , const DynamicMesh& mesh, const dtMeshTile* sourceTile, const int detailTriCount, const int detailVertCount); +static void WriteDetailMeshMixed (dtPolyDetail* detail, float* dverts, dtPolyDetailIndex* dtris + , const DynamicMesh& mesh, const dtMeshTile* sourceTile, const int detailTriCount, const int detailVertCount); +static void WriteOffMeshLinks (dtOffMeshConnection* offMeshCons, dtPoly* polys, float* verts, int polyCount, int vertCount, const dtMeshTile* sourceTile); +static int SimplePolygonTriangulation (dtPolyDetail* dtl, dtPolyDetailIndex* dtris, int detailTriBase, const int polygonVertexCount); + + + +// Converts detour navmesh tile to dynamic mesh format +bool TileToDynamicMesh (const dtMeshTile* tile, DynamicMesh& mesh, const Vector3f& tileOffset) +{ + if (!tile || !tile->header) + { + return false; + } + + const int vertCount = tile->header->vertCount; + const int polyCount = tile->header->polyCount; + mesh.Reserve (vertCount, polyCount); + + for (int iv = 0; iv < vertCount; ++iv) + { + const Vector3f srcVertex(&tile->verts[3*iv]); + mesh.AddVertex (srcVertex - tileOffset); + } + + for (int ip = 0; ip < polyCount; ++ip) + { + const dtPoly& srcPoly = tile->polys[ip]; + if (srcPoly.getType () == DT_POLYTYPE_GROUND) + mesh.AddPolygon (srcPoly.verts, ip, srcPoly.vertCount); + } + + return true; +} + +// Create tile in the format understood by the detour runtime. +// Polygons are converted from the dynamic mesh 'mesh'. +// Settings and static offmeshlinks are carried over from 'sourceTile'. +unsigned char* DynamicMeshToTile (int* dataSize, const DynamicMesh& mesh, const dtMeshTile* sourceTile, const Vector3f& tileOffset) +{ + // Determine data size + DebugAssert (sourceTile); + const int vertCount = mesh.VertCount (); + const int polyCount = mesh.PolyCount (); + const dtMeshHeader* sourceHeader = sourceTile->header; + + int polyEdgeCount = 0; + for (int ip = 0; ip < polyCount; ++ip) + { + polyEdgeCount += mesh.GetPoly (ip)->m_VertexCount; + } + + const int offMeshConCount = sourceHeader->offMeshConCount; + const int totVertCount = vertCount + 2 * offMeshConCount; + const int totPolyCount = polyCount + offMeshConCount; + const int totLinkCount = polyEdgeCount + 4*offMeshConCount; // TODO: reserve for links to external offmeshlink connections + + const bool hasHeightMesh = sourceTile->header->flags & DT_MESH_HEADER_USE_HEIGHT_MESH; + + int detailVertCount = 0; + int detailTriCount = 0; + if (hasHeightMesh) + { + RequirementsForDetailMeshUsingHeightMesh (&detailVertCount, &detailTriCount, mesh, sourceTile); + } else + { + RequirementsForDetailMeshMixed (&detailVertCount, &detailTriCount, mesh, sourceTile); + } + + const unsigned int headSize = dtAlign4 (sizeof (dtMeshHeader)); + const unsigned int vertSize = dtAlign4 (totVertCount * 3*sizeof (float)); + const unsigned int polySize = dtAlign4 (totPolyCount * sizeof (dtPoly)); + const unsigned int linkSize = dtAlign4 (totLinkCount * sizeof (dtLink)); + const unsigned int detailMeshesSize = dtAlign4 (polyCount * sizeof (dtPolyDetail)); + const unsigned int detailVertsSize = dtAlign4 (detailVertCount * 3*sizeof (float)); + const unsigned int detailTrisSize = dtAlign4 (detailTriCount * 4*sizeof (dtPolyDetailIndex)); + const unsigned int bvTreeSize = 0; + const unsigned int offMeshConsSize = dtAlign4 (offMeshConCount * sizeof (dtOffMeshConnection)); + + const int newSize = headSize + vertSize + polySize + linkSize + + detailTrisSize + detailVertsSize + detailMeshesSize + bvTreeSize + offMeshConsSize; + + unsigned char* newTile = dtAllocArray<unsigned char> (newSize); + if (newTile == NULL) + { + *dataSize = 0; + return NULL; + } + *dataSize = newSize; + memset (newTile, 0, newSize); + + // Serialize in the detour recognized format + int offset = 0; + dtMeshHeader* header = (dtMeshHeader*)(newTile+offset); offset += headSize; + float* verts = (float*)(newTile+offset); offset += vertSize; + dtPoly* polys = (dtPoly*)(newTile+offset); offset += polySize; + /*dtLink* links = (dtLink*)(newTile+offset);*/ offset += linkSize; + dtPolyDetail* detail = (dtPolyDetail*)(newTile+offset); offset += detailMeshesSize; + float* dverts = (float*)(newTile+offset); offset += detailVertsSize; + dtPolyDetailIndex* dtris = (dtPolyDetailIndex*)(newTile+offset); offset += detailTrisSize; + /*dtBVNode* bvtree = (dtBVNode*)(newTile+offset);*/ offset += bvTreeSize; + dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)(newTile+offset); offset += offMeshConsSize; + DebugAssert (offset == newSize); + + for (int iv = 0; iv < vertCount; ++iv) + { + const float* v = mesh.GetVertex (iv); + verts[3*iv+0] = v[0] + tileOffset.x; + verts[3*iv+1] = v[1] + tileOffset.y; + verts[3*iv+2] = v[2] + tileOffset.z; + } + + for (int ip = 0; ip < polyCount; ++ip) + { + const DynamicMesh::Poly* p = mesh.GetPoly (ip); + const int sourcePolyIndex = *mesh.GetData (ip); + const dtPoly& srcPoly = sourceTile->polys[sourcePolyIndex]; + + dtPoly& poly = polys[ip]; + memcpy (poly.verts, p->m_VertexIDs, DT_VERTS_PER_POLYGON*sizeof (UInt16)); + memcpy (poly.neis, p->m_Neighbours, DT_VERTS_PER_POLYGON*sizeof (UInt16)); + unsigned char area = srcPoly.getArea (); + poly.flags = 1<<area; + poly.setArea (area); + poly.setType (DT_POLYTYPE_GROUND); + poly.vertCount = p->m_VertexCount; + } + + // Set external portal flags + WritePortalFlags (verts, polys, polyCount, sourceHeader); + + if (hasHeightMesh) + { + WriteDetailMeshUsingHeightMesh (detail, dverts, dtris, mesh, sourceTile, detailTriCount, detailVertCount); + } else + { + WriteDetailMeshMixed (detail, dverts, dtris, mesh, sourceTile, detailTriCount, detailVertCount); + } + + // Fill in offmeshlink data from source tile: vertices, polygons, connection data. + WriteOffMeshLinks (offMeshCons, polys, verts, polyCount, vertCount, sourceTile); + + // Copy values from source + memcpy (header, sourceHeader, sizeof (*header)); + + // (re)set new tile values + header->polyCount = totPolyCount; + header->vertCount = totVertCount; + header->maxLinkCount = totLinkCount; + header->detailMeshCount = polyCount; + header->detailVertCount = detailVertCount; + header->detailTriCount = detailTriCount; + header->bvNodeCount = 0; // Fixme: bv-tree + header->offMeshConCount = offMeshConCount; + header->offMeshBase = polyCount; // points beyond regular polygons. + + return newTile; +} + +// Find vertex and triangle count needed when 'heightmesh' is enabled +static void RequirementsForDetailMeshUsingHeightMesh (int* detailVertCount, int* detailTriCount, const DynamicMesh& mesh, const dtMeshTile* sourceTile) +{ + // This is so bad - but until we change the detail mesh representation to + // something more sane - we'll have to write code like this. + int vertCount = 0; + int triCount = 0; + + // collect sizes needed for detail mesh + // detail mesh count is same as polyCount. + // they're 1-to-1 with the regular polygons + const int polyCount = mesh.PolyCount (); + for (int ip = 0; ip < polyCount; ++ip) + { + const int sourcePolyIndex = *mesh.GetData (ip); + const dtPolyDetail& sourceDetail = sourceTile->detailMeshes[sourcePolyIndex]; + + vertCount += sourceDetail.vertCount; + triCount += sourceDetail.triCount; + } + *detailVertCount = vertCount; + *detailTriCount = triCount; +} + +// Find vertex and triangle count needed for regular detailmesh +static void RequirementsForDetailMeshMixed (int* detailVertCount, int* detailTriCount, const DynamicMesh& mesh, const dtMeshTile* sourceTile) +{ + int vertCount = 0; + int triCount = 0; + + // Collect sizes needed for detail mesh + const int polyCount = mesh.PolyCount (); + for (int ip = 0; ip < polyCount; ++ip) + { + const DynamicMesh::Poly* p = mesh.GetPoly (ip); + const int sourcePolyIndex = *mesh.GetData (ip); + + if (p->m_Status == DynamicMesh::kOriginalPolygon) + { + // When preserving polygon detail mesh just add the source counts + const dtPolyDetail& sourceDetail = sourceTile->detailMeshes[sourcePolyIndex]; + vertCount += sourceDetail.vertCount; + triCount += sourceDetail.triCount; + } + else + { + // Simple triangulation needs n-2 triangles but no extra detail vertices + triCount += p->m_VertexCount - 2; + } + } + *detailVertCount = vertCount; + *detailTriCount = triCount; +} + +// Set flags on polygon edges colinear to tile edges. +// Flagged edges are considered when dynamically stitching neighboring tiles. +static void WritePortalFlags (const float* verts, dtPoly* polys, const int polyCount, const dtMeshHeader* sourceHeader) +{ + const float* bmax = sourceHeader->bmax; + const float* bmin = sourceHeader->bmin; + for (int ip = 0; ip < polyCount; ++ip) + { + dtPoly& poly = polys[ip]; + for (int iv = 0; iv < poly.vertCount; ++iv) + { + // Skip already connected edges + if (poly.neis[iv] != 0) + continue; + + const float* vert = &verts[3 * poly.verts[iv]]; + const int ivn = (iv+1 == poly.vertCount) ? 0 : iv+1; + const float* nextVert = &verts[3 * poly.verts[ivn]]; + + unsigned short nei = 0; + + if (dtMax (dtAbs (vert[0] - bmax[0]), dtAbs (nextVert[0] - bmax[0])) < MAGIC_EDGE_DISTANCE) + nei = DT_EXT_LINK | 0; // x+ portal + else if (dtMax (dtAbs (vert[2] - bmax[2]), dtAbs (nextVert[2] - bmax[2])) < MAGIC_EDGE_DISTANCE) + nei = DT_EXT_LINK | 2; // z+ portal + else if (dtMax (dtAbs (vert[0] - bmin[0]), dtAbs (nextVert[0] - bmin[0])) < MAGIC_EDGE_DISTANCE) + nei = DT_EXT_LINK | 4; // x- portal + else if (dtMax (dtAbs (vert[2] - bmin[2]), dtAbs (nextVert[2] - bmin[2])) < MAGIC_EDGE_DISTANCE) + nei = DT_EXT_LINK | 6; // z- portal + + poly.neis[iv] = nei; + } + } +} + +// Populate the tile with detail mesh. For the case where 'heightmesh' is enabled. +static void WriteDetailMeshUsingHeightMesh (dtPolyDetail* detail, float* dverts, dtPolyDetailIndex* dtris + , const DynamicMesh& mesh, const dtMeshTile* sourceTile + , const int detailTriCount, const int detailVertCount) +{ + int detailVertBase = 0; + int detailTriBase = 0; + + const int polyCount = mesh.PolyCount (); + for (int ip = 0; ip < polyCount; ++ip) + { + const int sourcePolyIndex = *mesh.GetData (ip); + const dtPolyDetail& sourceDetail = sourceTile->detailMeshes[sourcePolyIndex]; + + detail[ip].vertBase = detailVertBase; + detail[ip].triBase = detailTriBase; + detail[ip].triCount = sourceDetail.triCount; + detail[ip].vertCount = sourceDetail.vertCount; + + // Detail vertex indices are corrected by the poly vertex count + // adjust for this peculiarity. + // NOTE: It causes detail meshes to by unusable by multiple polygons ! + const int oldPolyVertexCount = sourceTile->polys[sourcePolyIndex].vertCount; + const int newPolyVertexCount = mesh.GetPoly (ip)->m_VertexCount; + const int vertexDelta = newPolyVertexCount - oldPolyVertexCount; + + for (int iv = 0; iv < sourceDetail.vertCount; ++iv) + { + dverts[3*(detailVertBase + iv) + 0] = sourceTile->detailVerts[3*(sourceDetail.vertBase + iv) + 0]; + dverts[3*(detailVertBase + iv) + 1] = sourceTile->detailVerts[3*(sourceDetail.vertBase + iv) + 1]; + dverts[3*(detailVertBase + iv) + 2] = sourceTile->detailVerts[3*(sourceDetail.vertBase + iv) + 2]; + } + + for (int it = 0; it < sourceDetail.triCount; ++it) + { + dtris[4*(detailTriBase + it) + 0] = sourceTile->detailTris[4*(sourceDetail.triBase + it) + 0] + vertexDelta; + dtris[4*(detailTriBase + it) + 1] = sourceTile->detailTris[4*(sourceDetail.triBase + it) + 1] + vertexDelta; + dtris[4*(detailTriBase + it) + 2] = sourceTile->detailTris[4*(sourceDetail.triBase + it) + 2] + vertexDelta; + dtris[4*(detailTriBase + it) + 3] = 0; + } + + detailVertBase += sourceDetail.vertCount; + detailTriBase += sourceDetail.triCount; + } + DebugAssert (detailVertBase == detailVertCount); + DebugAssert (detailTriBase == detailTriCount); +} + +// Mix preserved detail mesh for untouched polygons with simple triangulation for generated polygons +static void WriteDetailMeshMixed (dtPolyDetail* detail, float* dverts, dtPolyDetailIndex* dtris + , const DynamicMesh& mesh, const dtMeshTile* sourceTile + , const int detailTriCount, const int detailVertCount) +{ + int detailVertBase = 0; + int detailTriBase = 0; + + const int polyCount = mesh.PolyCount (); + for (int ip = 0; ip < polyCount; ++ip) + { + dtPolyDetail& dtl = detail[ip]; + const DynamicMesh::Poly* p = mesh.GetPoly (ip); + + if (p->m_Status == DynamicMesh::kOriginalPolygon) + { + // Fill in the original detail mesh for this polygon + const int sourcePolyIndex = *mesh.GetData (ip); + const dtPolyDetail& sourceDetail = sourceTile->detailMeshes[sourcePolyIndex]; + dtl.vertBase = detailVertBase; + dtl.vertCount = sourceDetail.vertCount; + dtl.triBase = detailTriBase; + dtl.triCount = sourceDetail.triCount; + + // copy source detail vertices and triangles + memcpy (&dverts[3*detailVertBase], &sourceTile->detailVerts[3*sourceDetail.vertBase], 3*sizeof (float)*sourceDetail.vertCount); + memcpy (&dtris[4*detailTriBase], &sourceTile->detailTris[4*sourceDetail.triBase], 4*sizeof (dtPolyDetailIndex)*sourceDetail.triCount); + + detailVertBase += sourceDetail.vertCount; + detailTriBase += sourceDetail.triCount; + } + else + { + detailTriBase = SimplePolygonTriangulation (&dtl, dtris, detailTriBase, p->m_VertexCount); + } + } + DebugAssert (detailTriBase == detailTriCount); + DebugAssert (detailVertBase == detailVertCount); +} + +// Populate the tile with static offmesh links from the source tile. +static void WriteOffMeshLinks (dtOffMeshConnection* offMeshCons, dtPoly* polys, float* verts, int polyCount, int vertCount, const dtMeshTile* sourceTile) +{ + const dtMeshHeader* sourceHeader = sourceTile->header; + const int offMeshConCount = sourceHeader->offMeshConCount; + if (offMeshConCount) + { + memcpy (&polys[polyCount], &sourceTile->polys[sourceHeader->offMeshBase], offMeshConCount * sizeof (dtPoly)); + memcpy (offMeshCons, sourceTile->offMeshCons, offMeshConCount * sizeof (dtOffMeshConnection)); + + // Vertex base for offmeshlinks is not stored in tile header + // Here we assume that offmeshlink vertices are stored as the section of vertices + // and that each offmeshlink stores two vertices. + const int sourceOffMeshVertBase = sourceHeader->vertCount - 2*sourceHeader->offMeshConCount; + + memcpy (&verts[3*vertCount], &sourceTile->verts[3*sourceOffMeshVertBase], 2*3*sizeof (float)); + + // Fixup internal index references + for (int i = 0; i < offMeshConCount; ++i) + { + // Vertex indices + dtPoly* poly = & polys[polyCount + i]; + poly->verts[0] = (unsigned short)(vertCount + 2*i+0); + poly->verts[1] = (unsigned short)(vertCount + 2*i+1); + + // Polygon index + dtOffMeshConnection* con = &offMeshCons[i]; + con->poly = (unsigned short) (polyCount + i); + } + } +} + +static int SimplePolygonTriangulation (dtPolyDetail* dtl, dtPolyDetailIndex* dtris, int detailTriBase, const int polygonVertexCount) +{ + dtl->vertBase = 0; + dtl->vertCount = 0; + dtl->triBase = (unsigned int)detailTriBase; + dtl->triCount = (dtPolyDetailIndex)(polygonVertexCount-2); + + // Triangulate polygon (local indices). + for (int j = 2; j < polygonVertexCount; ++j) + { + dtPolyDetailIndex* t = &dtris[4*detailTriBase]; + t[0] = 0; + t[1] = (dtPolyDetailIndex)(j-1); + t[2] = (dtPolyDetailIndex)j; + // Bit for each edge that belongs to poly boundary. + t[3] = (1<<2); + if (j == 2) t[3] |= (1<<0); + if (j == polygonVertexCount-1) t[3] |= (1<<4); + detailTriBase++; + } + return detailTriBase; +} diff --git a/Runtime/NavMesh/NavMeshTileConversion.h b/Runtime/NavMesh/NavMeshTileConversion.h new file mode 100644 index 0000000..2bf3fd2 --- /dev/null +++ b/Runtime/NavMesh/NavMeshTileConversion.h @@ -0,0 +1,11 @@ +#ifndef _NAVMESHTILECONVERSION_H_INCLUDED_ +#define _NAVMESHTILECONVERSION_H_INCLUDED_ + +struct dtMeshTile; +class DynamicMesh; +class Vector3f; + +bool TileToDynamicMesh (const dtMeshTile* tile, DynamicMesh& mesh, const Vector3f& tileOffset); +unsigned char* DynamicMeshToTile (int* dataSize, const DynamicMesh& mesh, const dtMeshTile* sourceTile, const Vector3f& tileOffset); + +#endif diff --git a/Runtime/NavMesh/NavMeshTypes.h b/Runtime/NavMesh/NavMeshTypes.h new file mode 100644 index 0000000..cc26075 --- /dev/null +++ b/Runtime/NavMesh/NavMeshTypes.h @@ -0,0 +1,76 @@ +#pragma once +#ifndef NAVMESH_TYPES_H_INCLUDED +#define NAVMESH_TYPES_H_INCLUDED + +#include "Runtime/Math/Vector3.h" +#include "Runtime/Math/Matrix4x4.h" +#include "Runtime/Scripting/Backend/ScriptingTypes.h" +#include "DetourReference.h" +class NavMeshPath; + +// Keep this enum in sync with the one defined in "NavMeshAgentBindings.txt" +enum ObstacleAvoidanceType +{ + kNoObstacleAvoidance = 0, + kLowQualityObstacleAvoidance = 1, + kMedQualityObstacleAvoidance = 2, + kGoodQualityObstacleAvoidance = 3, + kHighQualityObstacleAvoidance = 4 +}; + +// Keep this struct in sync with the one defined in "NavMeshBindings.txt" +struct NavMeshHit +{ + Vector3f position; + Vector3f normal; + float distance; + int mask; + int hit; +}; + +// Keep this struct in sync with the one defined in "NavMeshBindings.txt" +enum NavMeshPathStatus +{ + kPathComplete = 0, + kPathPartial = 1, + kPathInvalid = 2 +}; + +// Keep this enum in sync with the one defined in "NavMeshBindings.txt" +enum OffMeshLinkType +{ + kLinkTypeManual = 0, + kLinkTypeDropDown = 1, + kLinkTypeJumpAcross = 2 +}; + +// Keep this struct in sync with the one defined in "NavMeshBindings.txt" +struct OffMeshLinkData +{ + int m_Valid; + int m_Activated; + int m_InstanceID; + OffMeshLinkType m_LinkType; + Vector3f m_StartPos; + Vector3f m_EndPos; +}; + +// Used in: NavMeshBindings.txt, NavMeshAgentBindings.txt, NavMeshPathBindings.txt +struct MonoNavMeshPath +{ + MonoNavMeshPath () + : native (NULL) + , corners (SCRIPTING_NULL) + {} + + NavMeshPath* native; + ScriptingObjectPtr corners; +}; + +struct NavMeshCarveData +{ + Matrix4x4f transform; + Vector3f size; +}; + +#endif diff --git a/Runtime/NavMesh/NavigationModuleRegistration.cpp b/Runtime/NavMesh/NavigationModuleRegistration.cpp new file mode 100644 index 0000000..9bb7ed8 --- /dev/null +++ b/Runtime/NavMesh/NavigationModuleRegistration.cpp @@ -0,0 +1,40 @@ +#include "UnityPrefix.h" +#include "Runtime/BaseClasses/ClassRegistration.h" +#include "Runtime/Modules/ModuleRegistration.h" + +static void RegisterNavMeshClasses (ClassRegistrationContext& context) +{ + REGISTER_CLASS (NavMeshLayers) + REGISTER_CLASS (NavMesh) + REGISTER_CLASS (NavMeshAgent) + REGISTER_CLASS (NavMeshSettings) + REGISTER_CLASS (OffMeshLink) + REGISTER_CLASS (NavMeshObstacle) +} + +#if ENABLE_MONO || UNITY_WINRT +void ExportNavMeshBindings (); +void ExportNavMeshPathBindings (); +void ExportNavMeshAgentBindings (); +void ExportNavMeshObstacleBindings (); + +static void RegisterNavmeshICallModule () +{ +#if !INTERNAL_CALL_STRIPPING + ExportNavMeshBindings (); + ExportNavMeshPathBindings (); + ExportNavMeshAgentBindings (); + ExportNavMeshObstacleBindings (); +#endif +} +#endif + +extern "C" EXPORT_MODULE void RegisterModule_Navigation () +{ + ModuleRegistrationInfo info; + info.registerClassesCallback = &RegisterNavMeshClasses; +#if ENABLE_MONO || UNITY_WINRT + info.registerIcallsCallback = &RegisterNavmeshICallModule; +#endif + RegisterModuleInfo (info); +} diff --git a/Runtime/NavMesh/OffMeshLink.cpp b/Runtime/NavMesh/OffMeshLink.cpp new file mode 100644 index 0000000..c1646d0 --- /dev/null +++ b/Runtime/NavMesh/OffMeshLink.cpp @@ -0,0 +1,315 @@ +#include "UnityPrefix.h" +#include "OffMeshLink.h" +#include "NavMesh.h" +#include "NavMeshSettings.h" +#include "NavMeshManager.h" +#include "DetourNavMesh.h" +#include "NavMeshLayers.h" +#include "DetourCrowd.h" +#include "Runtime/Utilities/ValidateArgs.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" + +OffMeshLink::OffMeshLink (MemLabelId& label, ObjectCreationMode mode) +: Super (label, mode) +{ + m_NavMeshLayer = 0; + m_StaticPolyRef = 0; + m_DynamicPolyRef = 0; + m_ManagerHandle = -1; + m_CostOverride = -1.0f; + m_BiDirectional = true; + m_AutoUpdatePositions = false; + m_ShouldUpdateDynamic = false; + m_Activated = true; +} + +void OffMeshLink::Reset () +{ + Super::Reset (); +} + +void OffMeshLink::SmartReset () +{ + Super::SmartReset (); +#if UNITY_EDITOR + m_NavMeshLayer = GetGameObject ().GetNavMeshLayer (); +#endif +} + +OffMeshLink::~OffMeshLink () +{ + DisableDynamic (); +} + +void OffMeshLink::AddToManager () +{ + if (m_StaticPolyRef) + { + GetNavMeshSettings ().SetOffMeshPolyAccess (m_StaticPolyRef, m_Activated); + + // TODO: We really should be uging users to update here - but this warning breaks runtime tests because of unexpected log. + //WarningStringObject ("This OffMeshLink is static. To make it dynamic please re-bake the navmesh or reset this OffMeshLink component", this); + } + else + { + UpdatePositions (); + } +} + +void OffMeshLink::RemoveFromManager () +{ + if (m_StaticPolyRef) + GetNavMeshSettings ().SetOffMeshPolyAccess (m_StaticPolyRef, m_Activated); + + DisableDynamic (); +} + +template<class TransferFunc> +void OffMeshLink::Transfer (TransferFunc& transfer) +{ + Super::Transfer (transfer); + + transfer.SetVersion (2); + + TRANSFER (m_NavMeshLayer); //< Bake time only + + #if UNITY_EDITOR + // NavMeshLayer has been moved from the game object to OffMeshLink in Unity 4.0 + // This ensures that the value is pulled from the game object in AwakeFromLoad + if (transfer.IsOldVersion (1)) + { + m_NavMeshLayer = 0xFFFFFFFF; + } + #endif + + TRANSFER (m_Start); //< Bake time only + TRANSFER (m_End); //< Bake time only + + ///@TODO: Get rid of the static polygon reference when breaking backwards compatibility + /// all component-based offmeshlinks should be dynamic. + transfer.Transfer (m_StaticPolyRef, "m_DtPolyRef", kHideInEditorMask); + // from being copied when duplicating an OML + TRANSFER (m_CostOverride); //< Changes propagated to navmesh at runtime + + transfer.Align (); + TRANSFER (m_BiDirectional); //< Bake time only + TRANSFER (m_Activated); //< Changes propagated to navmesh at runtime + TRANSFER (m_AutoUpdatePositions); //< Changes propagated to navmesh at runtime + +} + +void OffMeshLink::AwakeFromLoad (AwakeFromLoadMode mode) +{ + Super::AwakeFromLoad (mode); + + #if UNITY_EDITOR + // NavMeshLayer has been moved from the game object to OffMeshLink in Unity 4.0 + // This ensures that the value is pulled from the game object in AwakeFromLoad + if (m_NavMeshLayer == 0xFFFFFFFF && GetGameObjectPtr ()) + { + m_NavMeshLayer = GetGameObject ().GetNavMeshLayer (); + } + #endif + + NavMeshSettings& settings = GetNavMeshSettings (); + + // Prioritize static polyref (baked) over dynamic + if (m_StaticPolyRef && !m_AutoUpdatePositions) + { + DisableDynamic (); + // The object instanceIDs may change from load to load. + // So storing the instanceID (of OffMeshLink) in baked data is not an option. + // + // Instead we attempt to set it given the polyref we got from the bake process. + // + // However: storing references (polyRef) to baked data in an object (OffMeshLink) is unsafe! + // eg: a user does not save the scene after baking navmesh - and later opens scene. + // Or: user disables an OffMeshLink, bakes navmesh, and re-enables offmeshlink. + + if (settings.SetOffMeshPolyInstanceID (m_StaticPolyRef, GetInstanceID ())) + { + settings.SetOffMeshPolyCostOverride (m_StaticPolyRef, m_CostOverride); + settings.SetOffMeshPolyAccess (m_StaticPolyRef, m_Activated); + } + } + else + { + m_ShouldUpdateDynamic = true; + } +} + +void OffMeshLink::UpdatePositions () +{ + if (!IsActive ()) + return; + + DisableStatic (); + DisableDynamic (false); + EnableDynamic (); + m_ShouldUpdateDynamic = false; +} + +void OffMeshLink::EnableDynamic () +{ +#if DT_DYNAMIC_OFFMESHLINK + if (m_ManagerHandle == -1) + GetNavMeshManager ().RegisterOffMeshLink (*this, m_ManagerHandle); + + if (m_DynamicPolyRef || !m_Activated || !m_End || !m_Start || m_NavMeshLayer == NavMeshLayers::kNotWalkable) + return; + + NavMeshSettings& settings = GetNavMeshSettings (); + dtNavMesh* navmesh = settings.GetInternalNavMesh (); + if (!navmesh) + return; + + const int instanceID = GetInstanceID (); + const Vector3f startPos = m_Start->GetPosition (); + const Vector3f endPos = m_End->GetPosition (); + + m_DynamicPolyRef = navmesh->AddDynamicOffMeshLink (startPos.GetPtr (), endPos.GetPtr (), instanceID, m_BiDirectional, (unsigned char)m_NavMeshLayer); + if (m_DynamicPolyRef) + { + settings.SetOffMeshPolyCostOverride (m_DynamicPolyRef, m_CostOverride); + } +#endif +} + +void OffMeshLink::CheckDynamicPositions () +{ +#if DT_DYNAMIC_OFFMESHLINK + if (m_DynamicPolyRef == 0 || m_ShouldUpdateDynamic) + return; + + NavMeshSettings& settings = GetNavMeshSettings (); + dtNavMesh* navmesh = settings.GetInternalNavMesh (); + if (!navmesh) + return; + + if (!m_Start || !m_End) + return; + + const Vector3f objectStartPos = m_Start->GetPosition (); + const Vector3f objectEndPos = m_End->GetPosition (); + const dtOffMeshConnection* con = navmesh->GetDynamicOffMeshLink (m_DynamicPolyRef); + if (con) + { + const float connectionRadius = con->rad; + const Vector3f startPos = Vector3f (&con->pos[0]); + const Vector3f endPos = Vector3f (&con->pos[3]); + m_ShouldUpdateDynamic = !CompareApproximately (startPos, objectStartPos, connectionRadius) || !CompareApproximately (endPos, objectEndPos, connectionRadius); + } +#endif +} + +void OffMeshLink::OnNavMeshCleanup () +{ + ClearDynamicPolyRef (); +} + +void OffMeshLink::OnNavMeshChanged () +{ + ClearDynamicPolyRef (); + m_ShouldUpdateDynamic = true; +} + +void OffMeshLink::UpdateMovedPositions () +{ + if (m_AutoUpdatePositions) + CheckDynamicPositions (); + + if (m_ShouldUpdateDynamic || m_DynamicPolyRef == 0) + { + UpdatePositions (); + } +} + +void OffMeshLink::DisableStatic () +{ + GetNavMeshSettings ().SetOffMeshPolyAccess (m_StaticPolyRef, false); +} + +void OffMeshLink::DisableDynamic (bool unregister) +{ +#if DT_DYNAMIC_OFFMESHLINK + if (unregister && m_ManagerHandle != -1) + GetNavMeshManager ().UnregisterOffMeshLink (m_ManagerHandle); + + if (m_DynamicPolyRef == 0) + return; + + if (dtNavMesh* navmesh = GetNavMeshSettings ().GetInternalNavMesh ()) + navmesh->RemoveDynamicOffMeshLink (m_DynamicPolyRef); + + m_DynamicPolyRef = 0; +#endif +} + +void OffMeshLink::SetCostOverride (float costOverride) +{ + ABORT_INVALID_FLOAT (costOverride, costOverride, OffMeshLink); + if (m_CostOverride == costOverride) + return; + + dtPolyRef ref = GetStaticOrDynamicPolyRef (); + GetNavMeshSettings ().SetOffMeshPolyCostOverride (ref, costOverride); + m_CostOverride = costOverride; + SetDirty (); +} + +void OffMeshLink::SetBiDirectional (bool bidirectional) +{ + if (m_BiDirectional == bidirectional) + return; + m_BiDirectional = bidirectional; + SetDirty (); +} + +void OffMeshLink::SetActivated (bool activated) +{ + if (m_Activated == activated) + return; + + m_Activated = activated; + + if (m_StaticPolyRef) + { + GetNavMeshSettings ().SetOffMeshPolyAccess (m_StaticPolyRef, activated); + } + else + { + if (activated && !m_DynamicPolyRef) + { + EnableDynamic (); + } + else if (!activated && m_DynamicPolyRef) + { + DisableDynamic (); + } + } + SetDirty (); +} + +void OffMeshLink::SetNavMeshLayer (UInt32 layer) +{ + if (m_NavMeshLayer == layer) + return; + + m_NavMeshLayer = layer; + UpdatePositions (); + SetDirty (); +} + + +bool OffMeshLink::GetOccupied () const +{ + if (const dtCrowd* crowd = GetNavMeshManager ().GetCrowdSystem ()) + { + const dtPolyRef ref = GetStaticOrDynamicPolyRef (); + return crowd->IsRefOccupied (ref); + } + return false; +} + +IMPLEMENT_CLASS (OffMeshLink) +IMPLEMENT_OBJECT_SERIALIZE (OffMeshLink) diff --git a/Runtime/NavMesh/OffMeshLink.h b/Runtime/NavMesh/OffMeshLink.h new file mode 100644 index 0000000..26d31cf --- /dev/null +++ b/Runtime/NavMesh/OffMeshLink.h @@ -0,0 +1,170 @@ +#ifndef RUNTIME_OFFMESHLINK +#define RUNTIME_OFFMESHLINK + +#include "Runtime/GameCode/Behaviour.h" +#include "Runtime/Graphics/Transform.h" +#include "NavMeshTypes.h" + +class dtNavMesh; + + + +class OffMeshLink : public Behaviour +{ +public: + REGISTER_DERIVED_CLASS (OffMeshLink, Behaviour) + DECLARE_OBJECT_SERIALIZE (OffMeshLink) + + OffMeshLink (MemLabelId& label, ObjectCreationMode mode); + // ~OffMeshLink (); declared by a macro + + inline void SetManagerHandle (int handle); + inline float GetCostOverride () const; + void SetCostOverride (float costOverride); + + inline bool GetBiDirectional () const; + void SetBiDirectional (bool bidirectional); + + inline bool GetActivated () const; + void SetActivated (bool activated); + + bool GetOccupied () const; + + inline UInt32 GetNavMeshLayer () const; + void SetNavMeshLayer (UInt32 layer); + + inline void ClearDynamicPolyRef (); + inline bool ClearStaticPolyRef (); + + inline Transform* GetStartTransform () const; + inline void SetStartTransform (Transform* t); + + inline Transform* GetEndTransform () const; + inline void SetEndTransform (Transform* t); + + void OnNavMeshCleanup (); + void OnNavMeshChanged (); + void UpdatePositions (); + void UpdateMovedPositions (); + + inline bool GetAutoUpdatePositions () const; + inline void SetAutoUpdatePositions (bool autoUpdate); + +protected: + virtual void AwakeFromLoad (AwakeFromLoadMode mode); + virtual void AddToManager (); + virtual void RemoveFromManager (); + virtual void Reset (); + virtual void SmartReset (); + +private: + void EnableDynamic (); + void DisableDynamic (bool unregister = true); + void DisableStatic (); + void CheckDynamicPositions (); + inline dtPolyRef GetStaticOrDynamicPolyRef () const; + + PPtr<Transform> m_Start; + PPtr<Transform> m_End; + + UInt32 m_NavMeshLayer; + bool m_AutoUpdatePositions; + bool m_ShouldUpdateDynamic; + + dtPolyRef m_StaticPolyRef; + dtPolyRef m_DynamicPolyRef; + int m_ManagerHandle; + float m_CostOverride; + bool m_BiDirectional; + bool m_Activated; +}; + + +inline void OffMeshLink::SetManagerHandle (int handle) +{ + m_ManagerHandle = handle; +} + +inline float OffMeshLink::GetCostOverride () const +{ + return m_CostOverride; +} + +inline bool OffMeshLink::GetBiDirectional () const +{ + return m_BiDirectional; +} + +inline bool OffMeshLink::GetActivated () const +{ + return m_Activated; +} + +inline UInt32 OffMeshLink::GetNavMeshLayer () const +{ + return m_NavMeshLayer; +} + +inline void OffMeshLink::ClearDynamicPolyRef () +{ + m_DynamicPolyRef = 0; +} + +inline bool OffMeshLink::ClearStaticPolyRef () +{ + if (m_StaticPolyRef == 0) + { + return false; + } + m_StaticPolyRef = 0; + SetDirty (); + return true; +} + +inline Transform* OffMeshLink::GetStartTransform () const +{ + return m_Start; +} + +inline void OffMeshLink::SetStartTransform (Transform* t) +{ + if (t == m_Start) + { + return; + } + m_Start = t; + SetDirty (); +} + +inline Transform* OffMeshLink::GetEndTransform () const +{ + return m_End; +} + +inline void OffMeshLink::SetEndTransform (Transform* t) +{ + if (t == m_End) + { + return; + } + m_End = t; + SetDirty (); +} + +inline bool OffMeshLink::GetAutoUpdatePositions () const +{ + return m_AutoUpdatePositions; +} + +inline void OffMeshLink::SetAutoUpdatePositions (bool autoUpdate) +{ + m_AutoUpdatePositions = autoUpdate; + SetDirty (); +} + +inline dtPolyRef OffMeshLink::GetStaticOrDynamicPolyRef () const +{ + return m_StaticPolyRef ? m_StaticPolyRef : m_DynamicPolyRef; +} + +#endif diff --git a/Runtime/NavMesh/ScriptBindings/NavMeshAgentBindings.txt b/Runtime/NavMesh/ScriptBindings/NavMeshAgentBindings.txt new file mode 100644 index 0000000..973c4b3 --- /dev/null +++ b/Runtime/NavMesh/ScriptBindings/NavMeshAgentBindings.txt @@ -0,0 +1,263 @@ +C++RAW +#include "UnityPrefix.h" +#include "Configuration/UnityConfigure.h" +#include "Runtime/Mono/MonoManager.h" +#include "Runtime/Mono/MonoBehaviour.h" +#include "Runtime/NavMesh/NavMesh.h" +#include "Runtime/NavMesh/NavMeshAgent.h" +#include "Runtime/NavMesh/OffMeshLink.h" +#include "Runtime/Scripting/ScriptingUtility.h" +#include "Runtime/Scripting/ScriptingExportUtility.h" + +CSRAW + +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace UnityEngine +{ + +// Keep this enum in sync with the one defined in "NavMeshTypes.h" +ENUM ObstacleAvoidanceType + // Disable avoidance. + NoObstacleAvoidance = 0, + + // Enable simple avoidance. Low performance impact. + LowQualityObstacleAvoidance = 1, + + // Medium avoidance. Medium performance impact + MedQualityObstacleAvoidance = 2, + + // Good avoidance. High performance impact + GoodQualityObstacleAvoidance = 3, + + // Enable highest precision. Highest performance impact. + HighQualityObstacleAvoidance = 4 +END + + + +// Navigation mesh agent. +CLASS NavMeshAgent : Behaviour + + // Sets or updates the destination. This triggers calculation for a new path. + CUSTOM bool SetDestination (Vector3 target) + { + return self->SetDestination (target); + } + + // Destination to navigate towards. + AUTO_PROP Vector3 destination GetDestination SetDestination + + // Stop within this distance from the target position. + AUTO_PROP float stoppingDistance GetStoppingDistance SetStoppingDistance + + // The current velocity of the [[NavMeshAgent]] component. + AUTO_PROP Vector3 velocity GetVelocity SetVelocity + + // The next position on the path. + AUTO_PROP Vector3 nextPosition GetNextPosition SetInternalAgentPosition + + // The current steering target - usually the next corner or end point of the current path. (RO) + AUTO_PROP Vector3 steeringTarget GetNextCorner + + // The desired velocity of the agent including any potential contribution from avoidance. (RO) + AUTO_PROP Vector3 desiredVelocity GetDesiredVelocity + + // Remaining distance along the current path - or infinity when not known. (RO) + AUTO_PROP float remainingDistance GetRemainingDistance + + // The relative vertical displacement of the owning [[GameObject]]. + AUTO_PROP float baseOffset GetBaseOffset SetBaseOffset + + // Is agent currently positioned on an OffMeshLink. (RO) + CUSTOM_PROP bool isOnOffMeshLink + { + return self->IsOnOffMeshLink (); + } + + // Enables or disables the current link. + CUSTOM void ActivateCurrentOffMeshLink (bool activated) + { + return self->ActivateCurrentOffMeshLink (activated); + } + + // The current [[OffMeshLinkData]]. + CSRAW public OffMeshLinkData currentOffMeshLinkData { get { return GetCurrentOffMeshLinkDataInternal (); } } + + CUSTOM internal OffMeshLinkData GetCurrentOffMeshLinkDataInternal () + { + OffMeshLinkData data; + self->GetCurrentOffMeshLinkData (&data); + return data; + } + + // The next [[OffMeshLinkData]] on the current path. + CSRAW public OffMeshLinkData nextOffMeshLinkData { get { return GetNextOffMeshLinkDataInternal (); } } + + CUSTOM internal OffMeshLinkData GetNextOffMeshLinkDataInternal () + { + OffMeshLinkData data; + self->GetNextOffMeshLinkData (&data); + return data; + } + + // Terminate OffMeshLink occupation and transfer the agent to the closest point on other side. + CUSTOM void CompleteOffMeshLink () + { + return self->CompleteOffMeshLink (); + } + + // Automate movement onto and off of OffMeshLinks. + AUTO_PROP bool autoTraverseOffMeshLink GetAutoTraverseOffMeshLink SetAutoTraverseOffMeshLink + + // Automate braking of NavMeshAgent to avoid overshooting the destination. + AUTO_PROP bool autoBraking GetAutoBraking SetAutoBraking + + // Attempt to acquire a new path if the existing path becomes invalid + AUTO_PROP bool autoRepath GetAutoRepath SetAutoRepath + + // Does this agent currently have a path. (RO) + AUTO_PROP bool hasPath HasPath + + // A path is being computed, but not yet ready. (RO) + AUTO_PROP bool pathPending PathPending + + // Is the current path stale. (RO) + AUTO_PROP bool isPathStale IsPathStale + + // Query the state of the current path. + AUTO_PROP NavMeshPathStatus pathStatus GetPathStatus + + //*undocumented* + AUTO_PROP Vector3 pathEndPosition GetEndPositionOfCurrentPath + + //*undocumented* + CUSTOM bool Warp (Vector3 newPosition) + { + return self->Warp(newPosition); + } + + // Apply relative movement to current position. + CUSTOM void Move (Vector3 offset) + { + return self->Move (offset); + } + + // Stop movement of this agent along its current path. + CUSTOM void Stop (bool stopUpdates = false) + { + return self->Stop (stopUpdates); + } + + // Resumes the movement along the current path. + CUSTOM void Resume () + { + return self->Resume (); + } + + // Clears the current path. Note that this agent will not start looking for a new path until SetDestination is called. + CUSTOM void ResetPath () + { + return self->ResetPath (); + } + + // Assign path to this agent. + CUSTOM bool SetPath (NavMeshPath path) + { + Scripting::RaiseIfNull (path); + MonoNavMeshPath monopath; + MarshallManagedStructIntoNative (path, &monopath); + return self->SetPath (monopath.native); + } + + // Set or get a copy of the current path. + CSRAW public NavMeshPath path { get { NavMeshPath path = new NavMeshPath (); CopyPathTo (path); return path; } set { if(value==null) throw new NullReferenceException(); SetPath (value); } } + + CUSTOM internal void CopyPathTo (NavMeshPath path) + { + Scripting::RaiseIfNull (path); + MonoNavMeshPath monopath; + MarshallManagedStructIntoNative (path, &monopath); + self->CopyPath (monopath.native); + } + + // Locate the closest NavMesh edge. + CUSTOM bool FindClosestEdge (out NavMeshHit hit) + { + return self->DistanceToEdge (hit); + } + + // Trace movement towards a target postion in the NavMesh. Without moving the agent. + CUSTOM bool Raycast (Vector3 targetPosition, out NavMeshHit hit) + { + return self->Raycast (targetPosition, hit); + } + + // Calculate a path to a specified point and store the resulting path. + CSRAW public bool CalculatePath (Vector3 targetPosition, NavMeshPath path) + { + path.ClearCorners (); + return CalculatePathInternal (targetPosition, path); + } + + CUSTOM private bool CalculatePathInternal (Vector3 targetPosition, NavMeshPath path) + { + MonoNavMeshPath monopath; + MarshallManagedStructIntoNative (path, &monopath); + int actualSize = self->CalculatePolygonPath (targetPosition, monopath.native); + return actualSize>0; + } + + // Sample a position along the current path. + CUSTOM bool SamplePathPosition (int passableMask, float maxDistance, out NavMeshHit hit) + { + return self->SamplePathPosition (passableMask, maxDistance, hit); + } + + // Sets the cost for traversing over geometry of the layer type. + CUSTOM void SetLayerCost (int layer, float cost) + { + self->SetLayerCost (layer, cost); + } + + // Gets the cost for traversing over geometry of the layer type. + CUSTOM float GetLayerCost (int layer) + { + return self->GetLayerCost (layer); + } + + // Specifies which NavMesh layers are passable (bitfield). Changing /walkableMask/ will make the path stale (see ::ref::isPathStale) + AUTO_PROP int walkableMask GetWalkableMask SetWalkableMask + + // Maximum movement speed. + AUTO_PROP float speed GetSpeed SetSpeed + + // Maximum rotation speed in (deg/s). + AUTO_PROP float angularSpeed GetAngularSpeed SetAngularSpeed + + // Maximum acceleration. + AUTO_PROP float acceleration GetAcceleration SetAcceleration + + // Should the agent update the transform position. + AUTO_PROP bool updatePosition GetUpdatePosition SetUpdatePosition + + // Should the agent update the transform orientation. + AUTO_PROP bool updateRotation GetUpdateRotation SetUpdateRotation + + // Agent avoidance radius. + AUTO_PROP float radius GetRadius SetRadius + + // Agent height. + AUTO_PROP float height GetHeight SetHeight + + // The level of quality of avoidance. + AUTO_PROP ObstacleAvoidanceType obstacleAvoidanceType GetObstacleAvoidanceType SetObstacleAvoidanceType + + // The avoidance priority level. + AUTO_PROP int avoidancePriority GetAvoidancePriority SetAvoidancePriority +END + +CSRAW } + diff --git a/Runtime/NavMesh/ScriptBindings/NavMeshBindings.txt b/Runtime/NavMesh/ScriptBindings/NavMeshBindings.txt new file mode 100644 index 0000000..5757557 --- /dev/null +++ b/Runtime/NavMesh/ScriptBindings/NavMeshBindings.txt @@ -0,0 +1,265 @@ +C++RAW + +#include "UnityPrefix.h" +#include "Configuration/UnityConfigure.h" +#include "Runtime/Mono/MonoManager.h" +#include "Runtime/Mono/MonoBehaviour.h" +#include "Runtime/NavMesh/NavMesh.h" +#include "Runtime/NavMesh/NavMeshAgent.h" +#include "Runtime/NavMesh/OffMeshLink.h" +#include "Runtime/NavMesh/NavMeshSettings.h" +#include "Runtime/NavMesh/NavMeshLayers.h" +#include "Runtime/Scripting/ScriptingUtility.h" +#include "Runtime/Scripting/ScriptingExportUtility.h" +#include "Runtime/Scripting/Backend/ScriptingTypeRegistry.h" +#include "Runtime/Scripting/Backend/ScriptingBackendApi.h" +#include "External/Recast/Detour/Include/DetourNavMeshQuery.h" +#include "Runtime/Scripting/Scripting.h" + +CSRAW + +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace UnityEngine +{ + +// Keep this enum in sync with the one defined in "NavMeshTypes.h" +// Link type specifier. +ENUM OffMeshLinkType + + // Manually specified type of link. + LinkTypeManual = 0, + + // Vertical drop. + LinkTypeDropDown = 1, + + // Horizontal jump. + LinkTypeJumpAcross = 2 +END + +// Keep this struct in sync with the one defined in "NavMeshTypes.h" +// State of OffMeshLink. +STRUCT OffMeshLinkData + CSRAW private int m_Valid; + CSRAW private int m_Activated; + CSRAW private int m_InstanceID; + CSRAW private OffMeshLinkType m_LinkType; + CSRAW private Vector3 m_StartPos; + CSRAW private Vector3 m_EndPos; + + // Is link valid (RO). + CSRAW public bool valid { get { return m_Valid != 0; } } + + // Is link active (RO). + CSRAW public bool activated { get { return m_Activated != 0; } } + + // Link type specifier (RO). + CSRAW public OffMeshLinkType linkType { get { return m_LinkType; } } + + // Link start world position (RO). + CSRAW public Vector3 startPos { get { return m_StartPos; } } + + // Link end world position (RO). + CSRAW public Vector3 endPos { get { return m_EndPos; } } + + // The [[OffMeshLink]] if the link type is a manually placed Offmeshlink (RO). + CSRAW public OffMeshLink offMeshLink { get { return GetOffMeshLinkInternal (m_InstanceID); } } + + CUSTOM internal OffMeshLink GetOffMeshLinkInternal (int instanceID) + { + return Scripting::ScriptingWrapperFor (dynamic_instanceID_cast<OffMeshLink*> (instanceID)); + } + +END + +// Keep this struct in sync with the one defined in "NavMeshTypes.h" +// Result information for NavMesh queries. +STRUCT NavMeshHit + CSRAW private Vector3 m_Position; + CSRAW private Vector3 m_Normal; + CSRAW private float m_Distance; + CSRAW private int m_Mask; + CSRAW private int m_Hit; + + // Position of hit. + CSRAW public Vector3 position { get { return m_Position; } set { m_Position = value; } } + + // Normal at the point of hit. + CSRAW public Vector3 normal { get { return m_Normal; } set { m_Normal = value; } } + + // Distance to the point of hit. + CSRAW public float distance { get { return m_Distance; } set { m_Distance = value; } } + + // Mask specifying NavMeshLayers at point of hit. + CSRAW public int mask { get { return m_Mask; } set { m_Mask = value; } } + + // Flag set when hit. + CSRAW public bool hit { get { return m_Hit != 0; } set { m_Hit = value ? 1 : 0; } } +END + + +// Contains data describing a triangulation of the navmesh +STRUCT NavMeshTriangulation + CSRAW public Vector3[] vertices; + CSRAW public int[] indices; + CSRAW public int[] layers; +END + + +// Navigation mesh. +CLASS NavMesh : Object + + // Trace a ray between two points on the NavMesh. + CUSTOM static bool Raycast (Vector3 sourcePosition, Vector3 targetPosition, out NavMeshHit hit, int passableMask) + { + NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh (); + if (navMesh == NULL) { + InvalidateNavMeshHit (hit); + return false; + } + const dtQueryFilter filter = dtQueryFilter::createFilterForIncludeFlags (passableMask); + return navMesh->Raycast (hit, sourcePosition, targetPosition, filter); + } + + // Calculate a path between two points and store the resulting path. + CSRAW public static bool CalculatePath (Vector3 sourcePosition, Vector3 targetPosition, int passableMask, NavMeshPath path) + { + path.ClearCorners (); + return CalculatePathInternal (sourcePosition, targetPosition, passableMask, path); + } + + CUSTOM internal private static bool CalculatePathInternal (Vector3 sourcePosition, Vector3 targetPosition, int passableMask, NavMeshPath path) + { + NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh (); + if (navMesh == NULL) + return false; + + MonoNavMeshPath monopath; + MarshallManagedStructIntoNative (path, &monopath); + + const dtQueryFilter filter = dtQueryFilter::createFilterForIncludeFlags (passableMask); + int actualSize = navMesh->CalculatePolygonPath (monopath.native, sourcePosition, targetPosition, filter); + return actualSize>0; + } + + // Locate the closest NavMesh edge from a point on the NavMesh. + CUSTOM static bool FindClosestEdge (Vector3 sourcePosition, out NavMeshHit hit, int passableMask) + { + NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh (); + if (navMesh == NULL) { + InvalidateNavMeshHit (hit); + return false; + } + const dtQueryFilter filter = dtQueryFilter::createFilterForIncludeFlags (passableMask); + return navMesh->DistanceToEdge (hit, sourcePosition, filter); + } + + // Sample the NavMesh closest to the point specified. + CUSTOM static bool SamplePosition (Vector3 sourcePosition, out NavMeshHit hit, float maxDistance, int allowedMask) + { + NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh (); + if (navMesh == NULL) { + InvalidateNavMeshHit (hit); + return false; + } + const dtQueryFilter filter = dtQueryFilter::createFilterForIncludeFlags (allowedMask); + return navMesh->SamplePosition (hit, sourcePosition, filter, maxDistance); + } + + // Sets the cost for traversing over geometry of the layer type on all agents. + CUSTOM static void SetLayerCost (int layer, float cost) + { + GetNavMeshLayers ().SetLayerCost (layer, cost); + } + + // Gets the cost for traversing over geometry of the layer type on all agents. + CUSTOM static float GetLayerCost (int layer) + { + return GetNavMeshLayers ().GetLayerCost (layer); + } + + // Returns the layer index for a named layer. + CUSTOM static int GetNavMeshLayerFromName (string layerName) + { + return GetNavMeshLayers ().GetNavMeshLayerFromName (layerName.AsUTF8()); + } + + CONDITIONAL !UNITY_FLASH && !UNITY_WINRT + CSRAW public static NavMeshTriangulation CalculateTriangulation () + { + NavMeshTriangulation tri = new NavMeshTriangulation (); + TriangulateInternal (ref tri.vertices, ref tri.indices, ref tri.layers); + return tri; + } + + CONDITIONAL !UNITY_FLASH && !UNITY_WINRT + CUSTOM internal private static void TriangulateInternal (ref Vector3[] vertices, ref int[] indices, ref int[] layers) + { + if (NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh ()) { + NavMesh::Triangulation triangulation; + navMesh->Triangulate (triangulation); + + vertices = CreateScriptingArray (triangulation.vertices.begin (), triangulation.vertices.size (), MONO_COMMON.vector3 ); + indices = CreateScriptingArray (triangulation.indices.begin (), triangulation.indices.size (), MONO_COMMON.int_32 ); + layers = CreateScriptingArray (triangulation.layers.begin (), triangulation.layers.size (), MONO_COMMON.int_32 ); + } else { + vertices = CreateEmptyStructArray (MONO_COMMON.vector3); + indices = CreateEmptyStructArray (MONO_COMMON.int_32); + layers = CreateEmptyStructArray (MONO_COMMON.int_32); + } + } + + //*undocumented* DEPRECATED + CONDITIONAL !UNITY_FLASH && !UNITY_WINRT + OBSOLETE warning use NavMesh.CalculateTriangulation() instead. + CUSTOM static void Triangulate (out Vector3[] vertices, out int[] indices) + { + if (NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh ()) { + NavMesh::Triangulation triangulation; + navMesh->Triangulate (triangulation); + if (vertices) { + *vertices = CreateScriptingArray (triangulation.vertices.begin(), triangulation.vertices.size (), MONO_COMMON.vector3 ); + } + if (indices) { + *indices = CreateScriptingArray (triangulation.indices.begin(), triangulation.indices.size (), MONO_COMMON.int_32 ); + } + } + } + //*undocumented* + CUSTOM static void AddOffMeshLinks () { } + //*undocumented* + CUSTOM static void RestoreNavMesh () { } + +END + +// Link allowing movement outside the planar navigation mesh. +CLASS OffMeshLink : Component + + // Is link active. + AUTO_PROP bool activated GetActivated SetActivated + + // Is link occupied. (RO) + AUTO_PROP bool occupied GetOccupied + + // Modify pathfinding cost for the link. + AUTO_PROP float costOverride GetCostOverride SetCostOverride + + AUTO_PROP bool biDirectional GetBiDirectional SetBiDirectional + + CUSTOM void UpdatePositions () { return self->UpdatePositions (); } + + AUTO_PROP int navMeshLayer GetNavMeshLayer SetNavMeshLayer + + AUTO_PROP bool autoUpdatePositions GetAutoUpdatePositions SetAutoUpdatePositions + + AUTO_PTR_PROP Transform startTransform GetStartTransform SetStartTransform + + AUTO_PTR_PROP Transform endTransform GetEndTransform SetEndTransform + +END + + +CSRAW } + diff --git a/Runtime/NavMesh/ScriptBindings/NavMeshObstacleBindings.txt b/Runtime/NavMesh/ScriptBindings/NavMeshObstacleBindings.txt new file mode 100644 index 0000000..4c71baf --- /dev/null +++ b/Runtime/NavMesh/ScriptBindings/NavMeshObstacleBindings.txt @@ -0,0 +1,40 @@ +C++RAW + +#include "UnityPrefix.h" +#include "Configuration/UnityConfigure.h" +#include "Runtime/Mono/MonoBehaviour.h" +#include "Runtime/NavMesh/NavMeshObstacle.h" +#include "Runtime/Scripting/ScriptingUtility.h" +#include "Runtime/Scripting/ScriptingExportUtility.h" + +CSRAW + +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace UnityEngine +{ + +// Navigation mesh obstacle. +CLASS NavMeshObstacle : Behaviour + + // Obstacle height. + AUTO_PROP float height GetHeight SetHeight + + // Obstacle radius. + AUTO_PROP float radius GetRadius SetRadius + + // Obstacle velocity. + AUTO_PROP Vector3 velocity GetVelocity SetVelocity + + CONDITIONAL ENABLE_NAVMESH_CARVING + AUTO_PROP bool carving GetCarving SetCarving + + CONDITIONAL ENABLE_NAVMESH_CARVING + AUTO_PROP float carvingMoveThreshold GetMoveThreshold SetMoveThreshold + +END + +CSRAW } + diff --git a/Runtime/NavMesh/ScriptBindings/NavMeshPathBindings.txt b/Runtime/NavMesh/ScriptBindings/NavMeshPathBindings.txt new file mode 100644 index 0000000..9f67a39 --- /dev/null +++ b/Runtime/NavMesh/ScriptBindings/NavMeshPathBindings.txt @@ -0,0 +1,133 @@ +C++RAW + +#include "UnityPrefix.h" +#include "Configuration/UnityConfigure.h" +#include "Runtime/Mono/MonoBehaviour.h" +#include "Runtime/NavMesh/NavMesh.h" +#include "Runtime/NavMesh/NavMeshPath.h" +#include "Runtime/NavMesh/NavMeshSettings.h" +#include "Runtime/Scripting/ScriptingUtility.h" +#include "Runtime/Scripting/ScriptingExportUtility.h" +#include "Runtime/Mono/MonoManager.h" + +CSRAW + +using System; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace UnityEngine +{ + +// Keep this enum in sync with the one defined in "NavMeshPath.h" +// Status of path. +ENUM NavMeshPathStatus + // The path terminates at the destination. + PathComplete = 0, + + // The path cannot reach the destination. + PathPartial = 1, + + // The path is invalid. + PathInvalid = 2 +END + +CSRAW +[StructLayout (LayoutKind.Sequential)] +// Path navigation. +CLASS NavMeshPath + CSRAW internal IntPtr m_Ptr; + CSRAW internal Vector3[] m_corners; + + C++RAW + +#if !UNITY_FLASH && !UNITY_WEBGL && !UNITY_WINRT + #define GET ExtractMonoObjectData<NavMeshPath*>(self) +#else + inline NavMeshPath* GetNativeNavMeshPath (ScriptingObjectPtr self) + { + MonoNavMeshPath managedpath; + MarshallManagedStructIntoNative (self, &managedpath); + return managedpath.native; + } + #define GET GetNativeNavMeshPath (self) +#endif + + + // NavMeshPath constructor. + CUSTOM NavMeshPath () + { + MonoNavMeshPath managedPath; + managedPath.native = new NavMeshPath (); + MarshallNativeStructIntoManaged (managedPath,self); + } + + THREAD_SAFE + CUSTOM private void DestroyNavMeshPath () + { + if (GET) + { + delete GET; + } + } + + CSRAW ~NavMeshPath () + { + DestroyNavMeshPath (); + m_Ptr = IntPtr.Zero; + } + + CUSTOM private Vector3[] CalculateCornersInternal () + { + NavMesh* navMesh = GetNavMeshSettings ().GetNavMesh (); + if (navMesh == NULL) + return CreateEmptyStructArray (GetMonoManager ().GetCommonClasses ().vector3); + + const int polygonCount = GET->GetPolygonCount (); + if (polygonCount == 0) + return CreateEmptyStructArray (GetMonoManager ().GetCommonClasses ().vector3); + + Vector3f* corners; + ALLOC_TEMP (corners, Vector3f, 2+polygonCount); + + NavMeshPath* path = GET; + int cornerCount = navMesh->CalculatePathCorners (corners, 2+polygonCount, *path); + if (cornerCount == 0) + return CreateEmptyStructArray (GetMonoManager ().GetCommonClasses ().vector3); + + return CreateScriptingArray<Vector3f>(corners, cornerCount, GetMonoManager ().GetCommonClasses ().vector3); + } + + CUSTOM private void ClearCornersInternal () + { + GET->SetPolygonCount (0); + } + + // Erase all corner points from path. + CSRAW public void ClearCorners () + { + ClearCornersInternal (); + m_corners = null; + } + + CSRAW private void CalculateCorners () + { + if (m_corners == null) + m_corners = CalculateCornersInternal (); + } + + // Corner points of path. (RO) + CSRAW public Vector3[] corners { get { CalculateCorners (); return m_corners;} } + + // Status of the path. (RO) + CUSTOM_PROP NavMeshPathStatus status + { + return GET->GetStatus (); + } + + C++RAW + #undef GET +END + +CSRAW } + |