Inna/CypherCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Core/MMaps: updated detour to be current with tc (still doesn't find the right path sometimes)

Browse Source
This commit is contained in:
hondacrx
2018-08-28 14:42:01 -04:00
parent 8c35e4f830
commit a187bfd169
7 changed files with 521 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Framework/RecastDetour/Detour/DetourNavMesh.cs
+13 -33
View File
@@ -35,12 +35,11 @@ using dtTileRef = System.UInt64;
public static partial class Detour public static partial class Detour
{ {
public const uint DT_SALT_BITS = 16; public const uint DT_SALT_BITS = 12;
public const uint DT_TILE_BITS = 28; public const uint DT_TILE_BITS = 21;
public const uint DT_POLY_BITS = 20; public const uint DT_POLY_BITS = 31;
} }
public static partial class Detour public static partial class Detour
{ {
public static bool overlapSlabs(float[] amin, float[] amax, float[] bmin, float[] bmax, float px, float py) public static bool overlapSlabs(float[] amin, float[] amax, float[] bmin, float[] bmax, float px, float py)
@@ -85,6 +84,7 @@ public static partial class Detour
return va[2]; return va[2];
return 0; return 0;
} }
public static float getSlabCoord(float[] va, int vaStart, int side) public static float getSlabCoord(float[] va, int vaStart, int side)
{ {
if (side == 0 || side == 4) if (side == 0 || side == 4)
@@ -94,7 +94,6 @@ public static partial class Detour
return 0; return 0;
} }
public static void calcSlabEndPoints(float[] va, int vaStart, float[] vb, int vbStart, float[] bmin, float[] bmax, int side) public static void calcSlabEndPoints(float[] va, int vaStart, float[] vb, int vbStart, float[] bmin, float[] bmax, int side)
{ {
if (side == 0 || side == 4) if (side == 0 || side == 4)
@@ -157,28 +156,6 @@ public static partial class Detour
} }
/* /*
dtNavMesh* dtAllocNavMesh()
{
void* mem = dtAlloc(sizeof(dtNavMesh), DT_ALLOC_PERM);
if (!mem) return 0;
return new(mem) dtNavMesh;
}
*/
// @par
///
/// This function will only free the memory for tiles with the #DT_TILE_FREE_DATA
/// flag set.
/*
void dtFreeNavMesh(dtNavMesh* navmesh)
{
if (!navmesh) return;
navmesh.~dtNavMesh();
dtFree(navmesh);
}
*/
//////////////////////////////////////////////////////////////////////////////////////////
/**
@class dtNavMesh @class dtNavMesh
The navigation mesh consists of one or more tiles defining three primary types of structural data: The navigation mesh consists of one or more tiles defining three primary types of structural data:
@@ -207,7 +184,7 @@ public static partial class Detour
@see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh @see dtNavMeshQuery, dtCreateNavMeshData, dtNavMeshCreateParams, #dtAllocNavMesh, #dtFreeNavMesh
*/ */
/// A navigation mesh based on tiles of convex polygons. // A navigation mesh based on tiles of convex polygons.
// @ingroup detour // @ingroup detour
public class dtNavMesh public class dtNavMesh
{ {
@@ -276,7 +253,7 @@ public static partial class Detour
{ {
dtPolyRef saltMask = (dtPolyRef)(1 << (int)DT_SALT_BITS) - 1; dtPolyRef saltMask = (dtPolyRef)(1 << (int)DT_SALT_BITS) - 1;
dtPolyRef tileMask = (dtPolyRef)((1 << (int)DT_TILE_BITS) - 1); dtPolyRef tileMask = (dtPolyRef)((1 << (int)DT_TILE_BITS) - 1);
dtPolyRef polyMask = (dtPolyRef)((1 << (int)DT_POLY_BITS) - 1); dtPolyRef polyMask = (dtPolyRef)((1ul << (int)DT_POLY_BITS) - 1);
salt = (uint)((polyRef >> (int)(DT_POLY_BITS + DT_TILE_BITS)) & saltMask); salt = (uint)((polyRef >> (int)(DT_POLY_BITS + DT_TILE_BITS)) & saltMask);
it = (uint)((polyRef >> (int)DT_POLY_BITS) & tileMask); it = (uint)((polyRef >> (int)DT_POLY_BITS) & tileMask);
ip = (uint)(polyRef & polyMask); ip = (uint)(polyRef & polyMask);
@@ -308,7 +285,7 @@ public static partial class Detour
/// @see #encodePolyId /// @see #encodePolyId
public uint decodePolyIdPoly(dtPolyRef polyRef) public uint decodePolyIdPoly(dtPolyRef polyRef)
{ {
dtPolyRef polyMask = (dtPolyRef)((1 << (int)DT_POLY_BITS) - 1); dtPolyRef polyMask = (dtPolyRef)((1ul << (int)DT_POLY_BITS) - 1);
return (uint)(polyRef & polyMask); return (uint)(polyRef & polyMask);
} }
@@ -1110,7 +1087,10 @@ public static partial class Detour
tile.flags = flags; tile.flags = flags;
connectIntLinks(tile); connectIntLinks(tile);
// Base off-mesh connections to their starting polygons and connect connections inside the tile.
baseOffMeshLinks(tile); baseOffMeshLinks(tile);
connectExtOffMeshLinks(tile, tile, -1);
// Create connections with neighbour tiles. // Create connections with neighbour tiles.
const int MAX_NEIS = 32; const int MAX_NEIS = 32;
@@ -1121,11 +1101,11 @@ public static partial class Detour
nneis = getTilesAt(header.x, header.y, neis, MAX_NEIS); nneis = getTilesAt(header.x, header.y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j) for (int j = 0; j < nneis; ++j)
{ {
if (neis[j] != tile) if (neis[j] == tile)
{ continue;
connectExtLinks(tile, neis[j], -1); connectExtLinks(tile, neis[j], -1);
connectExtLinks(neis[j], tile, -1); connectExtLinks(neis[j], tile, -1);
}
connectExtOffMeshLinks(tile, neis[j], -1); connectExtOffMeshLinks(tile, neis[j], -1);
connectExtOffMeshLinks(neis[j], tile, -1); connectExtOffMeshLinks(neis[j], tile, -1);
} }
Source/Framework/RecastDetour/Detour/DetourNavMeshBuilder.cs
+15 -43
View File
@@ -68,6 +68,18 @@ public static partial class Detour
DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, //< Add a vertex at every polygon edge crossing. DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, //< Add a vertex at every polygon edge crossing.
}; };
/// Options for dtNavMeshQuery::initSlicedFindPath and updateSlicedFindPath
enum dtFindPathOptions
{
DT_FINDPATH_ANY_ANGLE = 0x02, ///< use raycasts during pathfind to "shortcut" (raycast still consider costs)
};
/// Options for dtNavMeshQuery::raycast
enum dtRaycastOptions
{
DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost
};
/// Flags representing the type of a navigation mesh polygon. /// Flags representing the type of a navigation mesh polygon.
public enum dtPolyTypes public enum dtPolyTypes
{ {
@@ -85,31 +97,11 @@ public static partial class Detour
/// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.) /// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.)
public uint firstLink; public uint firstLink;
/// The indices of the polygon's vertices. // The indices of the polygon's vertices.
/// The actual vertices are located in dtMeshTile::verts. // The actual vertices are located in dtMeshTile::verts.
public ushort[] verts = new ushort[DT_VERTS_PER_POLYGON]; public ushort[] verts = new ushort[DT_VERTS_PER_POLYGON];
/// Packed data representing neighbor polygons references and flags for each edge. // Packed data representing neighbor polygons references and flags for each edge.
/*
@var ushort dtPoly::neis[DT_VERTS_PER_POLYGON]
@par
Each entry represents data for the edge starting at the vertex of the same index.
E.g. The entry at index n represents the edge data for vertex[n] to vertex[n+1].
A value of zero indicates the edge has no polygon connection. (It makes up the
border of the navigation mesh.)
The information can be extracted as follows:
@code
neighborRef = neis[n] & 0xff; // Get the neighbor polygon reference.
if (neis[n] & #DT_EX_LINK)
{
// The edge is an external (portal) edge.
}
@endcode
* */
public ushort[] neis = new ushort[DT_VERTS_PER_POLYGON]; public ushort[] neis = new ushort[DT_VERTS_PER_POLYGON];
/// The user defined polygon flags. /// The user defined polygon flags.
@@ -380,23 +372,6 @@ public static partial class Detour
public float[] bmax = new float[3]; //< The maximum bounds of the tile's AABB. [(x, y, z)] public float[] bmax = new float[3]; //< The maximum bounds of the tile's AABB. [(x, y, z)]
/// The bounding volume quantization factor. /// The bounding volume quantization factor.
/*
@var float dtMeshHeader::bvQuantFactor
@par
This value is used for converting between world and bounding volume coordinates.
For example:
@code
const float cs = 1.0f / tile.header.bvQuantFactor;
const dtBVNode* n = &tile.bvTree[i];
if (n.i >= 0)
{
// This is a leaf node.
float worldMinX = tile.header.bmin[0] + n.bmin[0]*cs;
float worldMinY = tile.header.bmin[0] + n.bmin[1]*cs;
// Etc...
}
@endcode */
public float bvQuantFactor; public float bvQuantFactor;
public int FromBytes(byte[] array, int start) public int FromBytes(byte[] array, int start)
@@ -557,8 +532,6 @@ public static partial class Detour
offMeshCons[i] = new dtOffMeshConnection(); offMeshCons[i] = new dtOffMeshConnection();
start = offMeshCons[i].FromBytes(array, start); start = offMeshCons[i].FromBytes(array, start);
} }
//flags = BitConverter.ToInt32(array, start);
//start += sizeof(int);
return start; return start;
} }
@@ -599,7 +572,6 @@ public static partial class Detour
{ {
bytes.AddRange(offMeshCons[i].ToBytes()); bytes.AddRange(offMeshCons[i].ToBytes());
} }
bytes.AddRange(BitConverter.GetBytes(flags));
return bytes.ToArray(); return bytes.ToArray();
} }
Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs
+464 -165
View File
@@ -36,6 +36,7 @@ using System;
using System.Diagnostics; using System.Diagnostics;
using dtPolyRef = System.UInt64; using dtPolyRef = System.UInt64;
using dtStatus = System.UInt32; using dtStatus = System.UInt32;
using System.Linq;
// Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter. // Define DT_VIRTUAL_QUERYFILTER if you wish to derive a custom filter from dtQueryFilter.
// On certain platforms indirect or virtual function call is expensive. The default // On certain platforms indirect or virtual function call is expensive. The default
@@ -46,7 +47,7 @@ using dtStatus = System.UInt32;
public static partial class Detour public static partial class Detour
{ {
const float H_SCALE = 0.999f; // Search heuristic scale. const float H_SCALE = 2.0f; // Search heuristic scale.
/// Defines polygon filtering and traversal costs for navigation mesh query operations. /// Defines polygon filtering and traversal costs for navigation mesh query operations.
// @ingroup detour // @ingroup detour
@@ -141,6 +142,33 @@ public static partial class Detour
} }
} }
/// Provides information about raycast hit
/// filled by dtNavMeshQuery::raycast
/// @ingroup detour
public class dtRaycastHit
{
/// The hit parameter. (FLT_MAX if no wall hit.)
public float t;
/// hitNormal The normal of the nearest wall hit. [(x, y, z)]
public float[] hitNormal = new float[3];
/// The index of the edge on the final polygon where the wall was hit.
public int hitEdgeIndex;
/// Pointer to an array of reference ids of the visited polygons. [opt]
public dtPolyRef[] path;
/// The number of visited polygons. [opt]
public int pathCount;
/// The maximum number of polygons the @p path array can hold.
public int maxPath;
/// The cost of the path until hit.
public float pathCost;
};
////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////
// @class dtNavMeshQuery // @class dtNavMeshQuery
@@ -175,6 +203,8 @@ public static partial class Detour
public float[] startPos = new float[3]; public float[] startPos = new float[3];
public float[] endPos = new float[3]; public float[] endPos = new float[3];
public dtQueryFilter filter; public dtQueryFilter filter;
public uint options;
public float raycastLimitSqr;
public void dtcsClear() public void dtcsClear()
{ {
@@ -185,10 +215,12 @@ public static partial class Detour
endRef = 0; endRef = 0;
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
startPos[i] = .0f; startPos[i] = 0f;
endPos[i] = .0f; endPos[i] = 0f;
} }
filter = null; filter = null;
options = 0;
raycastLimitSqr = 0f;
} }
} }
private dtQueryData m_query = new dtQueryData(); //< Sliced query state. private dtQueryData m_query = new dtQueryData(); //< Sliced query state.
@@ -853,61 +885,31 @@ public static partial class Detour
nearestRef = 0; nearestRef = 0;
// Get nearby polygons from proximity grid. dtFindNearestPolyQuery query = new dtFindNearestPolyQuery(this, center);
dtPolyRef[] polys = new dtPolyRef[128];
int polyCount = 0;
if (dtStatusFailed(queryPolygons(center, extents, filter, polys, ref polyCount, 128)))
return DT_FAILURE | DT_INVALID_PARAM;
// Find nearest polygon amongst the nearby polygons. dtStatus status = queryPolygons(center, extents, filter, query);
dtPolyRef nearest = 0; if (dtStatusFailed(status))
float nearestDistanceSqr = float.MaxValue; return status;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef polyRef = polys[i];
float[] closestPtPoly = new float[3];
float[] diff = new float[3];
bool posOverPoly = false;
float d = 0;
closestPointOnPoly(polyRef, center, closestPtPoly, ref posOverPoly);
// If a point is directly over a polygon and closer than nearestRef = query.nearestRef();
// climb height, favor that instead of straight line nearest point. // Only override nearestPt if we actually found a poly so the nearest point
dtVsub(diff, center, closestPtPoly); // is valid.
if (posOverPoly) if (nearestRef != 0)
{ dtVcopy(nearestPt, query.nearestPoint());
dtMeshTile tile = null;
dtPoly poly = null;
m_nav.getTileAndPolyByRefUnsafe(polys[i], ref tile, ref poly);
d = (float)(Math.Abs(diff[1]) - tile.header.walkableClimb);
d = d > 0 ? d * d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < nearestDistanceSqr)
{
//if (nearestPt != null)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = polyRef;
}
}
//if (nearestRef)
nearestRef = nearest;
return DT_SUCCESS; return DT_SUCCESS;
} }
/// Queries polygons within a tile. /// Queries polygons within a tile.
public int queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax, dtQueryFilter filter, dtPolyRef[] polys, int polyStart, int maxPolys) public void queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax, dtQueryFilter filter, dtFindNearestPolyQuery query)
{ {
Debug.Assert(m_nav != null); Debug.Assert(m_nav != null);
const int batchSize = 32;
dtPolyRef[] polyRefs = new dtPolyRef[batchSize];
dtPoly[] polys = new dtPoly[batchSize];
int n = 0;
if (tile.bvTree != null) if (tile.bvTree != null)
{ {
dtBVNode node = tile.bvTree[0]; dtBVNode node = tile.bvTree[0];
@@ -918,7 +920,7 @@ public static partial class Detour
float qfac = tile.header.bvQuantFactor; float qfac = tile.header.bvQuantFactor;
// Calculate quantized box // Calculate quantized box
ushort[] bmin = new ushort[3];//, bmax[3]; ushort[] bmin = new ushort[3];
ushort[] bmax = new ushort[3]; ushort[] bmax = new ushort[3];
// dtClamp query box to world box. // dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
@@ -937,7 +939,6 @@ public static partial class Detour
// Traverse tree // Traverse tree
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile); dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
int n = 0;
int nodeIndex = 0; int nodeIndex = 0;
while (nodeIndex < endIndex) while (nodeIndex < endIndex)
{ {
@@ -951,8 +952,18 @@ public static partial class Detour
dtPolyRef polyRef = polyRefBase | (uint)node.i; dtPolyRef polyRef = polyRefBase | (uint)node.i;
if (filter.passFilter(polyRef, tile, tile.polys[node.i])) if (filter.passFilter(polyRef, tile, tile.polys[node.i]))
{ {
if (n < maxPolys) polyRefs[n] = polyRef;
polys[polyStart + n++] = polyRef; polys[n] = tile.polys[node.i];
if (n == batchSize - 1)
{
query.process(tile, polys, polyRefs, batchSize);
n = 0;
}
else
{
n++;
}
} }
} }
@@ -966,14 +977,11 @@ public static partial class Detour
nodeIndex += escapeIndex; nodeIndex += escapeIndex;
} }
} }
return n;
} }
else else
{ {
float[] bmin = new float[3];//, bmax[3]; float[] bmin = new float[3];
float[] bmax = new float[3]; float[] bmax = new float[3];
int n = 0;
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile); dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
for (int i = 0; i < tile.header.polyCount; ++i) for (int i = 0; i < tile.header.polyCount; ++i)
{ {
@@ -999,14 +1007,28 @@ public static partial class Detour
} }
if (dtOverlapBounds(qmin, qmax, bmin, bmax)) if (dtOverlapBounds(qmin, qmax, bmin, bmax))
{ {
if (n < maxPolys) polyRefs[n] = polyRef;
polys[polyStart + n++] = polyRef; polys[n] = p;
if (n == batchSize - 1)
{
query.process(tile, polys, polyRefs, batchSize);
n = 0;
}
else
{
n++;
} }
} }
return n;
} }
} }
// Process the last polygons that didn't make a full batch.
if (n > 0)
query.process(tile, polys, polyRefs, n);
}
/// Finds polygons that overlap the search box. /// Finds polygons that overlap the search box.
/// @param[in] center The center of the search box. [(x, y, z)] /// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)] /// @param[in] extents The search distance along each axis. [(x, y, z)]
@@ -1024,11 +1046,11 @@ public static partial class Detour
/// be filled to capacity. The method of choosing which polygons from the /// be filled to capacity. The method of choosing which polygons from the
/// full set are included in the partial result set is undefined. /// full set are included in the partial result set is undefined.
/// ///
public dtStatus queryPolygons(float[] center, float[] extents, dtQueryFilter filter, dtPolyRef[] polys, ref int polyCount, int maxPolys) public dtStatus queryPolygons(float[] center, float[] extents, dtQueryFilter filter, dtFindNearestPolyQuery query)
{ {
Debug.Assert(m_nav != null); Debug.Assert(m_nav != null);
float[] bmin = new float[3];//, bmax[3]; float[] bmin = new float[3];
float[] bmax = new float[3]; float[] bmax = new float[3];
dtVsub(bmin, center, extents); dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents); dtVadd(bmax, center, extents);
@@ -1041,7 +1063,6 @@ public static partial class Detour
int MAX_NEIS = 32; int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS]; dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int n = 0;
for (int y = miny; y <= maxy; ++y) for (int y = miny; y <= maxy; ++y)
{ {
for (int x = minx; x <= maxx; ++x) for (int x = minx; x <= maxx; ++x)
@@ -1049,16 +1070,10 @@ public static partial class Detour
int nneis = m_nav.getTilesAt(x, y, neis, MAX_NEIS); int nneis = m_nav.getTilesAt(x, y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j) for (int j = 0; j < nneis; ++j)
{ {
n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys, n, maxPolys - n); queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
if (n >= maxPolys)
{
polyCount = n;
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
} }
} }
} }
}
polyCount = n;
return DT_SUCCESS; return DT_SUCCESS;
} }
@@ -1092,14 +1107,12 @@ public static partial class Detour
pathCount = 0; pathCount = 0;
if (startRef == 0 || endRef == 0) if (maxPath <= 0)
return DT_FAILURE | DT_INVALID_PARAM;
if (maxPath == 0)
return DT_FAILURE | DT_INVALID_PARAM; return DT_FAILURE | DT_INVALID_PARAM;
// Validate input // Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef)) if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef) ||
startPos == null || endPos == null || filter == null || maxPath <= 0 || path == null)
return DT_FAILURE | DT_INVALID_PARAM; return DT_FAILURE | DT_INVALID_PARAM;
if (startRef == endRef) if (startRef == endRef)
@@ -1124,7 +1137,7 @@ public static partial class Detour
dtNode lastBestNode = startNode; dtNode lastBestNode = startNode;
float lastBestNodeCost = startNode.total; float lastBestNodeCost = startNode.total;
dtStatus status = DT_SUCCESS; bool outOfNodes = false;
while (!m_openList.empty()) while (!m_openList.empty())
{ {
@@ -1176,10 +1189,15 @@ public static partial class Detour
if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly)) if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue; continue;
dtNode neighbourNode = m_nodePool.getNode(neighbourRef); // deal explicitly with crossing tile boundaries
byte crossSide = 0;
if (bestTile.links[i].side != 0xff)
crossSide = (byte)(bestTile.links[i].side >> 1);
dtNode neighbourNode = m_nodePool.getNode(neighbourRef, crossSide);
if (neighbourNode == null) if (neighbourNode == null)
{ {
status |= DT_OUT_OF_NODES; outOfNodes = true;
continue; continue;
} }
@@ -1236,7 +1254,7 @@ public static partial class Detour
neighbourNode.id = neighbourRef; neighbourNode.id = neighbourRef;
unchecked unchecked
{ {
neighbourNode.flags = (byte)(neighbourNode.flags & (byte)~dtNodeFlags.DT_NODE_CLOSED); neighbourNode.flags = (byte)(neighbourNode.flags & ~(byte)dtNodeFlags.DT_NODE_CLOSED);
} }
neighbourNode.cost = cost; neighbourNode.cost = cost;
neighbourNode.total = total; neighbourNode.total = total;
@@ -1262,41 +1280,58 @@ public static partial class Detour
} }
} }
dtStatus status = getPathToNode(lastBestNode, path, ref pathCount, maxPath);
if (lastBestNode.id != endRef) if (lastBestNode.id != endRef)
status |= DT_PARTIAL_RESULT; status |= DT_PARTIAL_RESULT;
// Reverse the path. if (outOfNodes)
dtNode prev = null; status |= DT_OUT_OF_NODES;
dtNode node = lastBestNode;
do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev);
prev = node;
node = next;
}
while (node != null);
// Store path
node = prev;
int n = 0;
do
{
path[n++] = node.id;
if (n >= maxPath)
{
status |= DT_BUFFER_TOO_SMALL;
break;
}
node = m_nodePool.getNodeAtIdx(node.pidx);
}
while (node != null);
pathCount = (uint)n;
return status; return status;
} }
dtStatus getPathToNode(dtNode endNode, dtPolyRef[] path, ref uint pathCount, int maxPath)
{
// Find the length of the entire path.
dtNode curNode = endNode;
int length = 0;
do
{
length++;
curNode = m_nodePool.getNodeAtIdx(curNode.pidx);
} while (curNode != null);
// If the path cannot be fully stored then advance to the last node we will be able to store.
curNode = endNode;
int writeCount;
for (writeCount = length; writeCount > maxPath; writeCount--)
{
//dtAssert(curNode);
curNode = m_nodePool.getNodeAtIdx(curNode.pidx);
}
// Write path
for (int i = writeCount - 1; i >= 0; i--)
{
//dtAssert(curNode);
path[i] = curNode.id;
curNode = m_nodePool.getNodeAtIdx(curNode.pidx);
}
//dtAssert(!curNode);
pathCount = (uint)Math.Min(length, maxPath);
if (length > maxPath)
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
return DT_SUCCESS;
}
///@} ///@}
// @name Sliced Pathfinding Functions // @name Sliced Pathfinding Functions
/// Common use case: /// Common use case:
@@ -1320,7 +1355,7 @@ public static partial class Detour
/// The @p filter pointer is stored and used for the duration of the sliced /// The @p filter pointer is stored and used for the duration of the sliced
/// path query. /// path query.
/// ///
public dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, float[] startPos, float[] endPos, dtQueryFilter filter) public dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef, float[] startPos, float[] endPos, dtQueryFilter filter, uint options)
{ {
Debug.Assert(m_nav != null); Debug.Assert(m_nav != null);
Debug.Assert(m_nodePool != null); Debug.Assert(m_nodePool != null);
@@ -1335,6 +1370,8 @@ public static partial class Detour
dtVcopy(m_query.startPos, startPos); dtVcopy(m_query.startPos, startPos);
dtVcopy(m_query.endPos, endPos); dtVcopy(m_query.endPos, endPos);
m_query.filter = filter; m_query.filter = filter;
m_query.options = options;
m_query.raycastLimitSqr = float.MaxValue;
if (startRef == 0 || endRef == 0) if (startRef == 0 || endRef == 0)
return DT_FAILURE | DT_INVALID_PARAM; return DT_FAILURE | DT_INVALID_PARAM;
@@ -1343,6 +1380,16 @@ public static partial class Detour
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef)) if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
return DT_FAILURE | DT_INVALID_PARAM; return DT_FAILURE | DT_INVALID_PARAM;
// trade quality with performance?
if ((options & (int)dtFindPathOptions.DT_FINDPATH_ANY_ANGLE) != 0)
{
// limiting to several times the character radius yields nice results. It is not sensitive
// so it is enough to compute it from the first tile.
dtMeshTile tile = m_nav.getTileByRef(startRef);
float agentRadius = tile.header.walkableRadius;
m_query.raycastLimitSqr = dtSqr(agentRadius * 50.0f); //DT_RAY_CAST_LIMIT_PROPORTIONS;
}
if (startRef == endRef) if (startRef == endRef)
{ {
m_query.status = DT_SUCCESS; m_query.status = DT_SUCCESS;
@@ -1384,6 +1431,9 @@ public static partial class Detour
return DT_FAILURE; return DT_FAILURE;
} }
dtRaycastHit rayHit = new dtRaycastHit();
rayHit.maxPath = 0;
int iter = 0; int iter = 0;
while (iter < maxIter && !m_openList.empty()) while (iter < maxIter && !m_openList.empty())
{ {
@@ -1421,13 +1471,21 @@ public static partial class Detour
// Get parent poly and tile. // Get parent poly and tile.
dtPolyRef parentRef = 0; dtPolyRef parentRef = 0;
dtPolyRef grandpaRef = 0;
dtMeshTile parentTile = null; dtMeshTile parentTile = null;
dtPoly parentPoly = null; dtPoly parentPoly = null;
dtNode parentNode = null;
if (bestNode.pidx != 0) if (bestNode.pidx != 0)
parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id; {
parentNode = m_nodePool.getNodeAtIdx(bestNode.pidx);
parentRef = parentNode.id;
if (parentNode.pidx != 0)
grandpaRef = m_nodePool.getNodeAtIdx(parentNode.pidx).id;
}
if (parentRef != 0) if (parentRef != 0)
{ {
if (dtStatusFailed(m_nav.getTileAndPolyByRef(parentRef, ref parentTile, ref parentPoly))) bool invalidParent = dtStatusFailed(m_nav.getTileAndPolyByRef(parentRef, ref parentTile, ref parentPoly));
if (invalidParent || (grandpaRef != 0 && !m_nav.isValidPolyRef(grandpaRef)))
{ {
// The polygon has disappeared during the sliced query, fail. // The polygon has disappeared during the sliced query, fail.
m_query.status = DT_FAILURE; m_query.status = DT_FAILURE;
@@ -1437,6 +1495,14 @@ public static partial class Detour
} }
} }
// decide whether to test raycast to previous nodes
bool tryLOS = false;
if ((m_query.options & (int)dtFindPathOptions.DT_FINDPATH_ANY_ANGLE) != 0)
{
if ((parentRef != 0) && (dtVdistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
tryLOS = true;
}
for (uint i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next) for (uint i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
{ {
dtPolyRef neighbourRef = bestTile.links[i].polyRef; dtPolyRef neighbourRef = bestTile.links[i].polyRef;
@@ -1461,6 +1527,10 @@ public static partial class Detour
continue; continue;
} }
// do not expand to nodes that were already visited from the same parent
if (neighbourNode.pidx != 0 && neighbourNode.pidx == bestNode.pidx)
continue;
// If the node is visited the first time, calculate node position. // If the node is visited the first time, calculate node position.
if (neighbourNode.flags == 0) if (neighbourNode.flags == 0)
{ {
@@ -1473,30 +1543,44 @@ public static partial class Detour
float cost = 0; float cost = 0;
float heuristic = 0; float heuristic = 0;
// Special case for last node. // raycast parent
if (neighbourRef == m_query.endRef) bool foundShortCut = false;
rayHit.pathCost = rayHit.t = 0;
if (tryLOS)
{ {
// Cost raycast(parentRef, parentNode.pos, neighbourNode.pos, m_query.filter, (int)dtRaycastOptions.DT_RAYCAST_USE_COSTS, rayHit, grandpaRef);
float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos, foundShortCut = rayHit.t >= 1.0f;
parentRef, parentTile, parentPoly, }
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
float endCost = m_query.filter.getCost(neighbourNode.pos, m_query.endPos,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, null, null);
cost = bestNode.cost + curCost + endCost; // update move cost
heuristic = 0; if (foundShortCut)
{
// shortcut found using raycast. Using shorter cost instead
cost = parentNode.cost + rayHit.pathCost;
} }
else else
{ {
// Cost // No shortcut found.
float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos, float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos,
parentRef, parentTile, parentPoly, parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly, bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly); neighbourRef, neighbourTile, neighbourPoly);
cost = bestNode.cost + curCost; cost = bestNode.cost + curCost;
}
// Special case for last node.
if (neighbourRef == m_query.endRef)
{
float endCost = m_query.filter.getCost(neighbourNode.pos, m_query.endPos,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, null, null);
cost = cost + endCost;
heuristic = 0;
}
else
{
heuristic = dtVdist(neighbourNode.pos, m_query.endPos) * H_SCALE; heuristic = dtVdist(neighbourNode.pos, m_query.endPos) * H_SCALE;
} }
@@ -1510,12 +1594,13 @@ public static partial class Detour
continue; continue;
// Add or update the node. // Add or update the node.
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode); neighbourNode.pidx = foundShortCut ? bestNode.pidx : m_nodePool.getNodeIdx(bestNode);
neighbourNode.id = neighbourRef; neighbourNode.id = neighbourRef;
//neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED); neighbourNode.flags = (byte)(neighbourNode.flags & ~(byte)(dtNodeFlags.DT_NODE_CLOSED | dtNodeFlags.DT_NODE_PARENT_DETACHED));
neighbourNode.dtcsClearFlag(dtNodeFlags.DT_NODE_CLOSED);
neighbourNode.cost = cost; neighbourNode.cost = cost;
neighbourNode.total = total; neighbourNode.total = total;
if (foundShortCut)
neighbourNode.flags = (byte)(neighbourNode.flags | (byte)dtNodeFlags.DT_NODE_PARENT_DETACHED);
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_OPEN) != 0) if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_OPEN) != 0)
{ {
@@ -1587,11 +1672,15 @@ public static partial class Detour
dtNode prev = null; dtNode prev = null;
dtNode node = m_query.lastBestNode; dtNode node = m_query.lastBestNode;
int prevRay = 0;
do do
{ {
dtNode next = m_nodePool.getNodeAtIdx(node.pidx); dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev); node.pidx = m_nodePool.getNodeIdx(prev);
prev = node; prev = node;
int nextRay = node.flags & (byte)dtNodeFlags.DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
node.flags = (byte)((node.flags & ~(byte)dtNodeFlags.DT_NODE_PARENT_DETACHED) | prevRay); // and store it in the reversed path's node
prevRay = nextRay;
node = next; node = next;
} }
while (node != null); while (node != null);
@@ -1599,14 +1688,38 @@ public static partial class Detour
// Store path // Store path
node = prev; node = prev;
do do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
dtStatus status = 0;
if ((node.flags & (byte)dtNodeFlags.DT_NODE_PARENT_DETACHED) != 0)
{
float t = 0;
float[] normal = new float[3];
uint m = 0;
dtPolyRef[] temp = new dtPolyRef[path.Length];
status = raycast(node.id, node.pos, next.pos, m_query.filter, ref t, normal, temp, ref m, maxPath - n);
for (var i = 0; i < path.Length - n; ++i)
path[n + i] = temp[i];
n += (int)m;
// raycast ends on poly boundary and the path might include the next poly boundary.
if (path[n - 1] == next.id)
n--; // remove to avoid duplicates
}
else
{ {
path[n++] = node.id; path[n++] = node.id;
if (n >= maxPath) if (n >= maxPath)
status = DT_BUFFER_TOO_SMALL;
}
if ((status & DT_STATUS_DETAIL_MASK) != 0)
{ {
m_query.status |= DT_BUFFER_TOO_SMALL; m_query.status |= status & DT_STATUS_DETAIL_MASK;
break; break;
} }
node = m_nodePool.getNodeAtIdx(node.pidx); node = next;
} }
while (node != null); while (node != null);
} }
@@ -1675,11 +1788,15 @@ public static partial class Detour
} }
// Reverse the path. // Reverse the path.
int prevRay = 0;
do do
{ {
dtNode next = m_nodePool.getNodeAtIdx(node.pidx); dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev); node.pidx = m_nodePool.getNodeIdx(prev);
prev = node; prev = node;
int nextRay = node.flags & (byte)dtNodeFlags.DT_NODE_PARENT_DETACHED; // keep track of whether parent is not adjacent (i.e. due to raycast shortcut)
node.flags = (byte)((node.flags & ~(byte)dtNodeFlags.DT_NODE_PARENT_DETACHED) | prevRay); // and store it in the reversed path's node
prevRay = nextRay;
node = next; node = next;
} }
while (node != null); while (node != null);
@@ -1687,14 +1804,37 @@ public static partial class Detour
// Store path // Store path
node = prev; node = prev;
do do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
dtStatus status = 0;
if ((node.flags & (byte)dtNodeFlags.DT_NODE_PARENT_DETACHED) != 0)
{
float t = 0;
float[] normal = new float[3];
uint m = 0;
dtPolyRef[] temp = new dtPolyRef[path.Length - n];
status = raycast(node.id, node.pos, next.pos, m_query.filter, ref t, normal, temp, ref m, maxPath - n);
for (var i = 0; i < path.Length - n; ++i)
path[n + i] = temp[i];
n += (int)m;
// raycast ends on poly boundary and the path might include the next poly boundary.
if (path[n - 1] == next.id)
n--; // remove to avoid duplicates
}
else
{ {
path[n++] = node.id; path[n++] = node.id;
if (n >= maxPath) if (n >= maxPath)
status = DT_BUFFER_TOO_SMALL;
}
if ((status & DT_STATUS_DETAIL_MASK) != 0)
{ {
m_query.status |= DT_BUFFER_TOO_SMALL; m_query.status |= status & DT_STATUS_DETAIL_MASK;
break; break;
} }
node = m_nodePool.getNodeAtIdx(node.pidx); node = next;
} }
while (node != null); while (node != null);
} }
@@ -1730,10 +1870,17 @@ public static partial class Detour
if (straightPathRefs != null) if (straightPathRefs != null)
straightPathRefs[straightPathCount] = polyRef; straightPathRefs[straightPathCount] = polyRef;
straightPathCount++; straightPathCount++;
// If reached end of path or there is no space to append more vertices, return.
if (flags == (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END || straightPathCount >= maxStraightPath) // If there is no space to append more vertices, return.
if (straightPathCount >= maxStraightPath)
{ {
return DT_SUCCESS | ((straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0); return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
}
// If reached end of path or there is no space to append more vertices, return.
if (flags == (byte)dtStraightPathFlags.DT_STRAIGHTPATH_END)
{
return DT_SUCCESS;
} }
} }
return DT_IN_PROGRESS; return DT_IN_PROGRESS;
@@ -2392,6 +2539,60 @@ public static partial class Detour
return DT_SUCCESS; return DT_SUCCESS;
} }
/// @par
///
/// This method is meant to be used for quick, short distance checks.
///
/// If the path array is too small to hold the result, it will be filled as
/// far as possible from the start postion toward the end position.
///
/// <b>Using the Hit Parameter (t)</b>
///
/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
/// the end position. In this case the path represents a valid corridor to the
/// end position and the value of @p hitNormal is undefined.
///
/// If the hit parameter is zero, then the start position is on the wall that
/// was hit and the value of @p hitNormal is undefined.
///
/// If 0 < t < 1.0 then the following applies:
///
/// @code
/// distanceToHitBorder = distanceToEndPosition * t
/// hitPoint = startPos + (endPos - startPos) * t
/// @endcode
///
/// <b>Use Case Restriction</b>
///
/// The raycast ignores the y-value of the end position. (2D check.) This
/// places significant limits on how it can be used. For example:
///
/// Consider a scene where there is a main floor with a second floor balcony
/// that hangs over the main floor. So the first floor mesh extends below the
/// balcony mesh. The start position is somewhere on the first floor. The end
/// position is on the balcony.
///
/// The raycast will search toward the end position along the first floor mesh.
/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
/// (no wall hit), meaning it reached the end position. This is one example of why
/// this method is meant for short distance checks.
///
public dtStatus raycast(dtPolyRef startRef, float[] startPos, float[] endPos, dtQueryFilter filter, ref float t, float[] hitNormal, dtPolyRef[] path, ref uint pathCount, int maxPath)
{
dtRaycastHit hit = new dtRaycastHit();
hit.path = path;
hit.maxPath = maxPath;
dtStatus status = raycast(startRef, startPos, endPos, filter, 0, hit);
t = hit.t;
dtVcopy(hitNormal, hit.hitNormal);
pathCount = (uint)hit.pathCount;
return status;
}
/// Casts a 'walkability' ray along the surface of the navigation mesh from /// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position. /// the start position toward the end position.
/// @param[in] startRef The reference id of the start polygon. /// @param[in] startRef The reference id of the start polygon.
@@ -2443,37 +2644,53 @@ public static partial class Detour
/// (no wall hit), meaning it reached the end position. This is one example of why /// (no wall hit), meaning it reached the end position. This is one example of why
/// this method is meant for short distance checks. /// this method is meant for short distance checks.
/// ///
public dtStatus raycast(dtPolyRef startRef, float[] startPos, float[] endPos, dtQueryFilter filter, ref float t, float[] hitNormal, dtPolyRef[] path, ref uint pathCount, int maxPath) public dtStatus raycast(dtPolyRef startRef, float[] startPos, float[] endPos, dtQueryFilter filter, uint options, dtRaycastHit hit, dtPolyRef prevRef = 0)
{ {
Debug.Assert(m_nav != null); Debug.Assert(m_nav != null);
t = 0; hit.t = 0;
//if (pathCount) hit.pathCount = 0;
pathCount = 0; hit.pathCost = 0;
// Validate input // Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef)) if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM; return DT_FAILURE | DT_INVALID_PARAM;
dtPolyRef curRef = startRef; if (prevRef != 0 && !m_nav.isValidPolyRef(prevRef))
float[] verts = new float[DT_VERTS_PER_POLYGON * 3]; return DT_FAILURE | DT_INVALID_PARAM;
float[] dir = new float[3];
float[] curPos = new float[3];
float[] lastPos = new float[3];
float[] verts = new float[DT_VERTS_PER_POLYGON * 3 + 3];
int n = 0; int n = 0;
hitNormal[0] = 0; dtVcopy(curPos, startPos);
hitNormal[1] = 0; dtVsub(dir, endPos, startPos);
hitNormal[2] = 0; dtVset(hit.hitNormal, 0, 0, 0);
dtStatus status = DT_SUCCESS; dtStatus status = DT_SUCCESS;
dtMeshTile prevTile = new dtMeshTile();
dtMeshTile nextTile;
dtPoly prevPoly = new dtPoly();
dtPoly nextPoly;
dtPolyRef curRef;
// The API input has been checked already, skip checking internal data.
curRef = startRef;
dtMeshTile tile = new dtMeshTile();
dtPoly poly = new dtPoly();
m_nav.getTileAndPolyByRefUnsafe(curRef, ref tile, ref poly);
nextTile = prevTile = tile;
nextPoly = prevPoly = poly;
if (prevRef != 0)
m_nav.getTileAndPolyByRefUnsafe(prevRef, ref prevTile, ref prevPoly);
while (curRef != 0) while (curRef != 0)
{ {
// Cast ray against current polygon. // Cast ray against current polygon.
// The API input has been cheked already, skip checking internal data.
dtMeshTile tile = null;
dtPoly poly = null;
m_nav.getTileAndPolyByRefUnsafe(curRef, ref tile, ref poly);
// Collect vertices. // Collect vertices.
int nv = 0; int nv = 0;
for (int i = 0; i < (int)poly.vertCount; ++i) for (int i = 0; i < (int)poly.vertCount; ++i)
@@ -2487,25 +2704,31 @@ public static partial class Detour
if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, ref tmin, ref tmax, ref segMin, ref segMax)) if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, ref tmin, ref tmax, ref segMin, ref segMax))
{ {
// Could not hit the polygon, keep the old t and report hit. // Could not hit the polygon, keep the old t and report hit.
pathCount = (uint)n; hit.pathCount = n;
return status; return status;
} }
hit.hitEdgeIndex = segMax;
// Keep track of furthest t so far. // Keep track of furthest t so far.
if (tmax > t) if (tmax > hit.t)
t = tmax; hit.t = tmax;
// Store visited polygons. // Store visited polygons.
if (n < maxPath) if (n < hit.maxPath)
path[n++] = curRef; hit.path[n++] = curRef;
else else
status |= DT_BUFFER_TOO_SMALL; status |= DT_BUFFER_TOO_SMALL;
// Ray end is completely inside the polygon. // Ray end is completely inside the polygon.
if (segMax == -1) if (segMax == -1)
{ {
t = float.MaxValue; hit.t = float.MaxValue;
//if (pathCount) hit.pathCount = n;
pathCount = (uint)n;
// add the cost
if ((options & (int)dtRaycastOptions.DT_RAYCAST_USE_COSTS) != 0)
hit.pathCost += filter.getCost(curPos, endPos, prevRef, prevTile, prevPoly, curRef, tile, poly, curRef, tile, poly);
return status; return status;
} }
@@ -2521,8 +2744,8 @@ public static partial class Detour
continue; continue;
// Get pointer to the next polygon. // Get pointer to the next polygon.
dtMeshTile nextTile = null; nextTile = new dtMeshTile();
dtPoly nextPoly = null; nextPoly = new dtPoly();
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref nextTile, ref nextPoly); m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref nextTile, ref nextPoly);
// Skip off-mesh connections. // Skip off-mesh connections.
@@ -2594,6 +2817,25 @@ public static partial class Detour
} }
} }
// add the cost
if ((options & (int)dtRaycastOptions.DT_RAYCAST_USE_COSTS) != 0)
{
// compute the intersection point at the furthest end of the polygon
// and correct the height (since the raycast moves in 2d)
dtVcopy(lastPos, curPos);
dtVmad(curPos, startPos, dir, hit.t);
int e1Start = segMax * 3;
int e2Start = ((segMax + 1) % nv) * 3;
float[] eDir = new float[3];
float[] diff = new float[3];
dtVsub(eDir, 0, verts, e2Start, verts, e1Start);
dtVsub(diff, 0, curPos, 0, verts, e1Start);
float s = dtSqr(eDir[0]) > dtSqr(eDir[2]) ? diff[0] / eDir[0] : diff[2] / eDir[2];
curPos[1] = verts[e1Start + 1] + eDir[1] * s;
hit.pathCost += filter.getCost(lastPos, curPos, prevRef, prevTile, prevPoly, curRef, tile, poly, nextRef, nextTile, nextPoly);
}
if (nextRef == 0) if (nextRef == 0)
{ {
// No neighbour, we hit a wall. // No neighbour, we hit a wall.
@@ -2607,22 +2849,25 @@ public static partial class Detour
int vbStart = b * 3; int vbStart = b * 3;
float dx = verts[vbStart + 0] - verts[vaStart + 0]; float dx = verts[vbStart + 0] - verts[vaStart + 0];
float dz = verts[vbStart + 2] - verts[vaStart + 2]; float dz = verts[vbStart + 2] - verts[vaStart + 2];
hitNormal[0] = dz; hit.hitNormal[0] = dz;
hitNormal[1] = 0; hit.hitNormal[1] = 0;
hitNormal[2] = -dx; hit.hitNormal[2] = -dx;
dtVnormalize(hitNormal); dtVnormalize(hit.hitNormal);
//if (pathCount) hit.pathCount = n;
pathCount = (uint)n;
return status; return status;
} }
// No hit, advance to neighbour polygon. // No hit, advance to neighbour polygon.
prevRef = curRef;
curRef = nextRef; curRef = nextRef;
prevTile = tile;
tile = nextTile;
prevPoly = poly;
poly = nextPoly;
} }
//if (pathCount) hit.pathCount = n;
pathCount = (uint)n;
return status; return status;
} }
@@ -3686,6 +3931,60 @@ public static partial class Detour
return m_nav; return m_nav;
} }
} }
public class dtFindNearestPolyQuery
{
dtNavMeshQuery m_query;
float[] m_center;
float m_nearestDistanceSqr;
dtPolyRef m_nearestRef;
float[] m_nearestPoint = new float[3];
public dtFindNearestPolyQuery(dtNavMeshQuery query, float[] center)
{
m_query = query;
m_center = center;
m_nearestDistanceSqr = float.MaxValue;
m_nearestRef = 0;
}
public dtPolyRef nearestRef() { return m_nearestRef; }
public float[] nearestPoint() { return m_nearestPoint; }
public void process(dtMeshTile tile, dtPoly[] polys, dtPolyRef[] refs, int count)
{
for (int i = 0; i < count; ++i)
{
dtPolyRef refe = refs[i];
float[] closestPtPoly = new float[3];
float[] diff = new float[3];
bool posOverPoly = false;
float d;
m_query.closestPointOnPoly(refe, m_center, closestPtPoly, ref posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
dtVsub(diff, m_center, closestPtPoly);
if (posOverPoly)
{
d = Math.Abs(diff[1]) - tile.header.walkableClimb;
d = d > 0 ? d * d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < m_nearestDistanceSqr)
{
dtVcopy(m_nearestPoint, closestPtPoly);
m_nearestDistanceSqr = d;
m_nearestRef = refe;
}
}
}
}
} }
Source/Framework/RecastDetour/Detour/DetourNode.cs
+7 -5
View File
@@ -23,6 +23,7 @@ public partial class Detour
{ {
DT_NODE_OPEN = 0x01, DT_NODE_OPEN = 0x01,
DT_NODE_CLOSED = 0x02, DT_NODE_CLOSED = 0x02,
DT_NODE_PARENT_DETACHED = 0x04, // parent of the node is not adjacent. Found using raycast.
}; };
public const dtNodeIndex DT_NULL_IDX = dtNodeIndex.MaxValue; //(dtNodeIndex)~0; public const dtNodeIndex DT_NULL_IDX = dtNodeIndex.MaxValue; //(dtNodeIndex)~0;
@@ -32,8 +33,9 @@ public partial class Detour
public float[] pos = new float[3]; //< Position of the node. public float[] pos = new float[3]; //< Position of the node.
public float cost; //< Cost from previous node to current node. public float cost; //< Cost from previous node to current node.
public float total; //< Cost up to the node. public float total; //< Cost up to the node.
public uint pidx;// : 30; //< Index to parent node. public uint pidx;// : 24; //< Index to parent node.
public byte flags;// : 2; //< Node flags 0/open/closed. public uint state;// : 2; ///< extra state information. A polyRef can have multiple nodes with different extra info. see DT_MAX_STATES_PER_NODE
public byte flags;// : 3; //< Node flags 0/open/closed.
public dtPolyRef id; //< Polygon ref the node corresponds to. public dtPolyRef id; //< Polygon ref the node corresponds to.
/// ///
public static int getSizeOf() public static int getSizeOf()
@@ -73,7 +75,6 @@ public partial class Detour
////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////
public dtNodePool(int maxNodes, int hashSize) public dtNodePool(int maxNodes, int hashSize)
{ {
m_maxNodes = maxNodes; m_maxNodes = maxNodes;
m_hashSize = hashSize; m_hashSize = hashSize;
@@ -164,14 +165,14 @@ public partial class Detour
return null; return null;
} }
public dtNode getNode(dtPolyRef id) public dtNode getNode(dtPolyRef id, byte state = 0)
{ {
uint bucket = (uint)(dtHashRef(id) & (m_hashSize - 1)); uint bucket = (uint)(dtHashRef(id) & (m_hashSize - 1));
dtNodeIndex i = m_first[bucket]; dtNodeIndex i = m_first[bucket];
dtNode node = null; dtNode node = null;
while (i != DT_NULL_IDX) while (i != DT_NULL_IDX)
{ {
if (m_nodes[i].id == id) if (m_nodes[i].id == id && m_nodes[i].state == state)
return m_nodes[i]; return m_nodes[i];
i = m_next[i]; i = m_next[i];
} }
@@ -188,6 +189,7 @@ public partial class Detour
node.cost = 0; node.cost = 0;
node.total = 0; node.total = 0;
node.id = id; node.id = id;
node.state = state;
node.flags = 0; node.flags = 0;
m_next[i] = m_first[bucket]; m_next[i] = m_first[bucket];
Source/Game/Chat/Commands/MMapsCommands.cs
+1 -1
View File
@@ -172,7 +172,7 @@ namespace Game.Chat
for (int i = 0; i < navmesh.getMaxTiles(); ++i) for (int i = 0; i < navmesh.getMaxTiles(); ++i)
{ {
Detour.dtMeshTile tile = navmesh.getTile(i); Detour.dtMeshTile tile = navmesh.getTile(i);
if (tile == null) if (tile.header == null)
continue; continue;
handler.SendSysMessage("[{0:D2}, {1:D2}]", tile.header.x, tile.header.y); handler.SendSysMessage("[{0:D2}, {1:D2}]", tile.header.x, tile.header.y);
Source/Game/Maps/MMapManager.cs
+1 -5
View File
@@ -16,12 +16,9 @@
*/ */
using Framework.Constants; using Framework.Constants;
using Game.DataStorage;
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.IO; using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text; using System.Text;
namespace Game namespace Game
@@ -152,13 +149,12 @@ namespace Game
} }
var bytes = reader.ReadBytes((int)fileHeader.size); var bytes = reader.ReadBytes((int)fileHeader.size);
Detour.dtRawTileData data = new Detour.dtRawTileData(); Detour.dtRawTileData data = new Detour.dtRawTileData();
data.FromBytes(bytes, 0); data.FromBytes(bytes, 0);
ulong tileRef = 0; ulong tileRef = 0;
// memory allocated for data is now managed by detour, and will be deallocated when the tile is removed // memory allocated for data is now managed by detour, and will be deallocated when the tile is removed
if (Detour.dtStatusSucceed(mmap.navMesh.addTile(data, 0, 0, ref tileRef))) if (Detour.dtStatusSucceed(mmap.navMesh.addTile(data, 1, 0, ref tileRef)))
{ {
mmap.loadedTileRefs.Add(packedGridPos, tileRef); mmap.loadedTileRefs.Add(packedGridPos, tileRef);
++loadedTiles; ++loadedTiles;
Source/Game/Movement/Generators/PathGenerator.cs
+11 -7
View File
@@ -340,6 +340,7 @@ namespace Game.Movement
// generate suffix // generate suffix
uint suffixPolyLength = 0; uint suffixPolyLength = 0;
ulong[] tempPolyRefs = new ulong[_pathPolyRefs.Length];
uint dtResult; uint dtResult;
if (_straightLine) if (_straightLine)
@@ -354,7 +355,7 @@ namespace Game.Movement
_filter, _filter,
ref hit, ref hit,
hitNormal, hitNormal,
_pathPolyRefs, tempPolyRefs,
ref suffixPolyLength, ref suffixPolyLength,
74 - (int)prefixPolyLength); 74 - (int)prefixPolyLength);
@@ -374,7 +375,7 @@ namespace Game.Movement
suffixEndPoint, // start position suffixEndPoint, // start position
endPoint, // end position endPoint, // end position
_filter, // polygon search filter _filter, // polygon search filter
_pathPolyRefs, tempPolyRefs,
ref suffixPolyLength, ref suffixPolyLength,
74 - (int)prefixPolyLength); 74 - (int)prefixPolyLength);
} }
@@ -389,6 +390,9 @@ namespace Game.Movement
Log.outDebug(LogFilter.Maps, "m_polyLength={0} prefixPolyLength={1} suffixPolyLength={2} \n", _polyLength, prefixPolyLength, suffixPolyLength); Log.outDebug(LogFilter.Maps, "m_polyLength={0} prefixPolyLength={1} suffixPolyLength={2} \n", _polyLength, prefixPolyLength, suffixPolyLength);
for (var i = 0; i < _pathPolyRefs.Length - (prefixPolyLength - 1); ++i)
_pathPolyRefs[(prefixPolyLength - 1) + i] = tempPolyRefs[i];
// new path = prefix + suffix - overlap // new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1; _polyLength = prefixPolyLength + suffixPolyLength - 1;
} }
@@ -797,21 +801,21 @@ namespace Game.Movement
void CreateFilter() void CreateFilter()
{ {
NavArea includeFlags = 0; NavTerrainFlag includeFlags = 0;
NavArea excludeFlags = 0; NavTerrainFlag excludeFlags = 0;
if (_sourceUnit.IsTypeId(TypeId.Unit)) if (_sourceUnit.IsTypeId(TypeId.Unit))
{ {
Creature creature = _sourceUnit.ToCreature(); Creature creature = _sourceUnit.ToCreature();
if (creature.CanWalk()) if (creature.CanWalk())
includeFlags |= NavArea.Ground; includeFlags |= NavTerrainFlag.Ground;
// creatures don't take environmental damage // creatures don't take environmental damage
if (creature.CanSwim()) if (creature.CanSwim())
includeFlags |= (NavArea.Water | NavArea.MagmaSlime); includeFlags |= (NavTerrainFlag.Water | NavTerrainFlag.MagmaSlime);
} }
else else
includeFlags = (NavArea.Ground | NavArea.Water | NavArea.MagmaSlime); includeFlags = (NavTerrainFlag.Ground | NavTerrainFlag.Water | NavTerrainFlag.MagmaSlime);
_filter.setIncludeFlags((ushort)includeFlags); _filter.setIncludeFlags((ushort)includeFlags);
_filter.setExcludeFlags((ushort)excludeFlags); _filter.setExcludeFlags((ushort)excludeFlags);