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 const uint DT_SALT_BITS = 16;
public const uint DT_TILE_BITS = 28;
public const uint DT_POLY_BITS = 20;
public const uint DT_SALT_BITS = 12;
public const uint DT_TILE_BITS = 21;
public const uint DT_POLY_BITS = 31;
}
public static partial class Detour
{
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 0;
}
public static float getSlabCoord(float[] va, int vaStart, int side)
{
if (side == 0 || side == 4)
@@ -94,7 +94,6 @@ public static partial class Detour
return 0;
}
public static void calcSlabEndPoints(float[] va, int vaStart, float[] vb, int vbStart, float[] bmin, float[] bmax, int side)
{
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
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
*/
/// A navigation mesh based on tiles of convex polygons.
// A navigation mesh based on tiles of convex polygons.
// @ingroup detour
public class dtNavMesh
{
@@ -276,7 +253,7 @@ public static partial class Detour
{
dtPolyRef saltMask = (dtPolyRef)(1 << (int)DT_SALT_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);
it = (uint)((polyRef >> (int)DT_POLY_BITS) & tileMask);
ip = (uint)(polyRef & polyMask);
@@ -308,7 +285,7 @@ public static partial class Detour
/// @see #encodePolyId
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);
}
@@ -1110,7 +1087,10 @@ public static partial class Detour
tile.flags = flags;
connectIntLinks(tile);
// Base off-mesh connections to their starting polygons and connect connections inside the tile.
baseOffMeshLinks(tile);
connectExtOffMeshLinks(tile, tile, -1);
// Create connections with neighbour tiles.
const int MAX_NEIS = 32;
@@ -1121,11 +1101,11 @@ public static partial class Detour
nneis = getTilesAt(header.x, header.y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
if (neis[j] != tile)
{
if (neis[j] == tile)
continue;
connectExtLinks(tile, neis[j], -1);
connectExtLinks(neis[j], tile, -1);
}
connectExtOffMeshLinks(tile, neis[j], -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.
};
/// 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.
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.)
public uint firstLink;
/// The indices of the polygon's vertices.
/// The actual vertices are located in dtMeshTile::verts.
// The indices of the polygon's vertices.
// The actual vertices are located in dtMeshTile::verts.
public ushort[] verts = new ushort[DT_VERTS_PER_POLYGON];
/// 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
* */
// Packed data representing neighbor polygons references and flags for each edge.
public ushort[] neis = new ushort[DT_VERTS_PER_POLYGON];
/// 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)]
/// 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 int FromBytes(byte[] array, int start)
@@ -557,8 +532,6 @@ public static partial class Detour
offMeshCons[i] = new dtOffMeshConnection();
start = offMeshCons[i].FromBytes(array, start);
}
//flags = BitConverter.ToInt32(array, start);
//start += sizeof(int);
return start;
}
@@ -599,7 +572,6 @@ public static partial class Detour
{
bytes.AddRange(offMeshCons[i].ToBytes());
}
bytes.AddRange(BitConverter.GetBytes(flags));
return bytes.ToArray();
}
Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs
+464 -165
View File
@@ -36,6 +36,7 @@ using System;
using System.Diagnostics;
using dtPolyRef = System.UInt64;
using dtStatus = System.UInt32;
using System.Linq;
// 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
@@ -46,7 +47,7 @@ using dtStatus = System.UInt32;
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.
// @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
@@ -175,6 +203,8 @@ public static partial class Detour
public float[] startPos = new float[3];
public float[] endPos = new float[3];
public dtQueryFilter filter;
public uint options;
public float raycastLimitSqr;
public void dtcsClear()
{
@@ -185,10 +215,12 @@ public static partial class Detour
endRef = 0;
for (int i = 0; i < 3; ++i)
{
startPos[i] = .0f;
endPos[i] = .0f;
startPos[i] = 0f;
endPos[i] = 0f;
}
filter = null;
options = 0;
raycastLimitSqr = 0f;
}
}
private dtQueryData m_query = new dtQueryData(); //< Sliced query state.
@@ -853,61 +885,31 @@ public static partial class Detour
nearestRef = 0;
// Get nearby polygons from proximity grid.
dtPolyRef[] polys = new dtPolyRef[128];
int polyCount = 0;
if (dtStatusFailed(queryPolygons(center, extents, filter, polys, ref polyCount, 128)))
return DT_FAILURE | DT_INVALID_PARAM;
dtFindNearestPolyQuery query = new dtFindNearestPolyQuery(this, center);
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = float.MaxValue;
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);
dtStatus status = queryPolygons(center, extents, filter, query);
if (dtStatusFailed(status))
return status;
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
dtVsub(diff, center, closestPtPoly);
if (posOverPoly)
{
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;
nearestRef = query.nearestRef();
// Only override nearestPt if we actually found a poly so the nearest point
// is valid.
if (nearestRef != 0)
dtVcopy(nearestPt, query.nearestPoint());
return DT_SUCCESS;
}
/// 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);
const int batchSize = 32;
dtPolyRef[] polyRefs = new dtPolyRef[batchSize];
dtPoly[] polys = new dtPoly[batchSize];
int n = 0;
if (tile.bvTree != null)
{
dtBVNode node = tile.bvTree[0];
@@ -918,7 +920,7 @@ public static partial class Detour
float qfac = tile.header.bvQuantFactor;
// Calculate quantized box
ushort[] bmin = new ushort[3];//, bmax[3];
ushort[] bmin = new ushort[3];
ushort[] bmax = new ushort[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
@@ -937,7 +939,6 @@ public static partial class Detour
// Traverse tree
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
int n = 0;
int nodeIndex = 0;
while (nodeIndex < endIndex)
{
@@ -951,8 +952,18 @@ public static partial class Detour
dtPolyRef polyRef = polyRefBase | (uint)node.i;
if (filter.passFilter(polyRef, tile, tile.polys[node.i]))
{
if (n < maxPolys)
polys[polyStart + n++] = polyRef;
polyRefs[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;
}
}
return n;
}
else
{
float[] bmin = new float[3];//, bmax[3];
float[] bmin = new float[3];
float[] bmax = new float[3];
int n = 0;
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
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 (n < maxPolys)
polys[polyStart + n++] = polyRef;
polyRefs[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.
/// @param[in] center The center of the search box. [(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
/// 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);
float[] bmin = new float[3];//, bmax[3];
float[] bmin = new float[3];
float[] bmax = new float[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
@@ -1041,7 +1063,6 @@ public static partial class Detour
int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int n = 0;
for (int y = miny; y <= maxy; ++y)
{
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);
for (int j = 0; j < nneis; ++j)
{
n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys, n, maxPolys - n);
if (n >= maxPolys)
{
polyCount = n;
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
}
}
}
}
polyCount = n;
return DT_SUCCESS;
}
@@ -1092,14 +1107,12 @@ public static partial class Detour
pathCount = 0;
if (startRef == 0 || endRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
if (maxPath == 0)
if (maxPath <= 0)
return DT_FAILURE | DT_INVALID_PARAM;
// 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;
if (startRef == endRef)
@@ -1124,7 +1137,7 @@ public static partial class Detour
dtNode lastBestNode = startNode;
float lastBestNodeCost = startNode.total;
dtStatus status = DT_SUCCESS;
bool outOfNodes = false;
while (!m_openList.empty())
{
@@ -1176,10 +1189,15 @@ public static partial class Detour
if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
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)
{
status |= DT_OUT_OF_NODES;
outOfNodes = true;
continue;
}
@@ -1236,7 +1254,7 @@ public static partial class Detour
neighbourNode.id = neighbourRef;
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.total = total;
@@ -1262,41 +1280,58 @@ public static partial class Detour
}
}
dtStatus status = getPathToNode(lastBestNode, path, ref pathCount, maxPath);
if (lastBestNode.id != endRef)
status |= DT_PARTIAL_RESULT;
// Reverse the path.
dtNode prev = null;
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;
if (outOfNodes)
status |= DT_OUT_OF_NODES;
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
/// 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
/// 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_nodePool != null);
@@ -1335,6 +1370,8 @@ public static partial class Detour
dtVcopy(m_query.startPos, startPos);
dtVcopy(m_query.endPos, endPos);
m_query.filter = filter;
m_query.options = options;
m_query.raycastLimitSqr = float.MaxValue;
if (startRef == 0 || endRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
@@ -1343,6 +1380,16 @@ public static partial class Detour
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
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)
{
m_query.status = DT_SUCCESS;
@@ -1384,6 +1431,9 @@ public static partial class Detour
return DT_FAILURE;
}
dtRaycastHit rayHit = new dtRaycastHit();
rayHit.maxPath = 0;
int iter = 0;
while (iter < maxIter && !m_openList.empty())
{
@@ -1421,13 +1471,21 @@ public static partial class Detour
// Get parent poly and tile.
dtPolyRef parentRef = 0;
dtPolyRef grandpaRef = 0;
dtMeshTile parentTile = null;
dtPoly parentPoly = null;
dtNode parentNode = null;
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 (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.
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)
{
dtPolyRef neighbourRef = bestTile.links[i].polyRef;
@@ -1461,6 +1527,10 @@ public static partial class Detour
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 (neighbourNode.flags == 0)
{
@@ -1473,30 +1543,44 @@ public static partial class Detour
float cost = 0;
float heuristic = 0;
// Special case for last node.
if (neighbourRef == m_query.endRef)
// raycast parent
bool foundShortCut = false;
rayHit.pathCost = rayHit.t = 0;
if (tryLOS)
{
// Cost
float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos,
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);
raycast(parentRef, parentNode.pos, neighbourNode.pos, m_query.filter, (int)dtRaycastOptions.DT_RAYCAST_USE_COSTS, rayHit, grandpaRef);
foundShortCut = rayHit.t >= 1.0f;
}
cost = bestNode.cost + curCost + endCost;
heuristic = 0;
// update move cost
if (foundShortCut)
{
// shortcut found using raycast. Using shorter cost instead
cost = parentNode.cost + rayHit.pathCost;
}
else
{
// Cost
// No shortcut found.
float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
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;
}
@@ -1510,12 +1594,13 @@ public static partial class Detour
continue;
// Add or update the node.
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
neighbourNode.pidx = foundShortCut ? bestNode.pidx : m_nodePool.getNodeIdx(bestNode);
neighbourNode.id = neighbourRef;
//neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
neighbourNode.dtcsClearFlag(dtNodeFlags.DT_NODE_CLOSED);
neighbourNode.flags = (byte)(neighbourNode.flags & ~(byte)(dtNodeFlags.DT_NODE_CLOSED | dtNodeFlags.DT_NODE_PARENT_DETACHED));
neighbourNode.cost = cost;
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)
{
@@ -1587,11 +1672,15 @@ public static partial class Detour
dtNode prev = null;
dtNode node = m_query.lastBestNode;
int prevRay = 0;
do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev);
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;
}
while (node != null);
@@ -1599,14 +1688,38 @@ public static partial class Detour
// Store path
node = prev;
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;
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;
}
node = m_nodePool.getNodeAtIdx(node.pidx);
node = next;
}
while (node != null);
}
@@ -1675,11 +1788,15 @@ public static partial class Detour
}
// Reverse the path.
int prevRay = 0;
do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev);
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;
}
while (node != null);
@@ -1687,14 +1804,37 @@ public static partial class Detour
// Store path
node = prev;
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;
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;
}
node = m_nodePool.getNodeAtIdx(node.pidx);
node = next;
}
while (node != null);
}
@@ -1730,10 +1870,17 @@ public static partial class Detour
if (straightPathRefs != null)
straightPathRefs[straightPathCount] = polyRef;
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;
@@ -2392,6 +2539,60 @@ public static partial class Detour
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
/// the start position toward the end position.
/// @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
/// 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);
t = 0;
//if (pathCount)
pathCount = 0;
hit.t = 0;
hit.pathCount = 0;
hit.pathCost = 0;
// Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
dtPolyRef curRef = startRef;
float[] verts = new float[DT_VERTS_PER_POLYGON * 3];
if (prevRef != 0 && !m_nav.isValidPolyRef(prevRef))
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;
hitNormal[0] = 0;
hitNormal[1] = 0;
hitNormal[2] = 0;
dtVcopy(curPos, startPos);
dtVsub(dir, endPos, startPos);
dtVset(hit.hitNormal, 0, 0, 0);
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)
{
// 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.
int nv = 0;
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))
{
// Could not hit the polygon, keep the old t and report hit.
pathCount = (uint)n;
hit.pathCount = n;
return status;
}
hit.hitEdgeIndex = segMax;
// Keep track of furthest t so far.
if (tmax > t)
t = tmax;
if (tmax > hit.t)
hit.t = tmax;
// Store visited polygons.
if (n < maxPath)
path[n++] = curRef;
if (n < hit.maxPath)
hit.path[n++] = curRef;
else
status |= DT_BUFFER_TOO_SMALL;
// Ray end is completely inside the polygon.
if (segMax == -1)
{
t = float.MaxValue;
//if (pathCount)
pathCount = (uint)n;
hit.t = float.MaxValue;
hit.pathCount = 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;
}
@@ -2521,8 +2744,8 @@ public static partial class Detour
continue;
// Get pointer to the next polygon.
dtMeshTile nextTile = null;
dtPoly nextPoly = null;
nextTile = new dtMeshTile();
nextPoly = new dtPoly();
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref nextTile, ref nextPoly);
// 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)
{
// No neighbour, we hit a wall.
@@ -2607,22 +2849,25 @@ public static partial class Detour
int vbStart = b * 3;
float dx = verts[vbStart + 0] - verts[vaStart + 0];
float dz = verts[vbStart + 2] - verts[vaStart + 2];
hitNormal[0] = dz;
hitNormal[1] = 0;
hitNormal[2] = -dx;
dtVnormalize(hitNormal);
hit.hitNormal[0] = dz;
hit.hitNormal[1] = 0;
hit.hitNormal[2] = -dx;
dtVnormalize(hit.hitNormal);
//if (pathCount)
pathCount = (uint)n;
hit.pathCount = n;
return status;
}
// No hit, advance to neighbour polygon.
prevRef = curRef;
curRef = nextRef;
prevTile = tile;
tile = nextTile;
prevPoly = poly;
poly = nextPoly;
}
//if (pathCount)
pathCount = (uint)n;
hit.pathCount = n;
return status;
}
@@ -3686,6 +3931,60 @@ public static partial class Detour
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_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;
@@ -32,8 +33,9 @@ public partial class Detour
public float[] pos = new float[3]; //< Position of the node.
public float cost; //< Cost from previous node to current node.
public float total; //< Cost up to the node.
public uint pidx;// : 30; //< Index to parent node.
public byte flags;// : 2; //< Node flags 0/open/closed.
public uint pidx;// : 24; //< Index to parent node.
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 static int getSizeOf()
@@ -73,7 +75,6 @@ public partial class Detour
//////////////////////////////////////////////////////////////////////////////////////////
public dtNodePool(int maxNodes, int hashSize)
{
m_maxNodes = maxNodes;
m_hashSize = hashSize;
@@ -164,14 +165,14 @@ public partial class Detour
return null;
}
public dtNode getNode(dtPolyRef id)
public dtNode getNode(dtPolyRef id, byte state = 0)
{
uint bucket = (uint)(dtHashRef(id) & (m_hashSize - 1));
dtNodeIndex i = m_first[bucket];
dtNode node = null;
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];
i = m_next[i];
}
@@ -188,6 +189,7 @@ public partial class Detour
node.cost = 0;
node.total = 0;
node.id = id;
node.state = state;
node.flags = 0;
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)
{
Detour.dtMeshTile tile = navmesh.getTile(i);
if (tile == null)
if (tile.header == null)
continue;
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 Game.DataStorage;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
namespace Game
@@ -152,13 +149,12 @@ namespace Game
}
var bytes = reader.ReadBytes((int)fileHeader.size);
Detour.dtRawTileData data = new Detour.dtRawTileData();
data.FromBytes(bytes, 0);
ulong tileRef = 0;
// 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);
++loadedTiles;
Source/Game/Movement/Generators/PathGenerator.cs
+11 -7
View File
@@ -340,6 +340,7 @@ namespace Game.Movement
// generate suffix
uint suffixPolyLength = 0;
ulong[] tempPolyRefs = new ulong[_pathPolyRefs.Length];
uint dtResult;
if (_straightLine)
@@ -354,7 +355,7 @@ namespace Game.Movement
_filter,
ref hit,
hitNormal,
_pathPolyRefs,
tempPolyRefs,
ref suffixPolyLength,
74 - (int)prefixPolyLength);
@@ -374,7 +375,7 @@ namespace Game.Movement
suffixEndPoint, // start position
endPoint, // end position
_filter, // polygon search filter
_pathPolyRefs,
tempPolyRefs,
ref suffixPolyLength,
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);
for (var i = 0; i < _pathPolyRefs.Length - (prefixPolyLength - 1); ++i)
_pathPolyRefs[(prefixPolyLength - 1) + i] = tempPolyRefs[i];
// new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
@@ -797,21 +801,21 @@ namespace Game.Movement
void CreateFilter()
{
NavArea includeFlags = 0;
NavArea excludeFlags = 0;
NavTerrainFlag includeFlags = 0;
NavTerrainFlag excludeFlags = 0;
if (_sourceUnit.IsTypeId(TypeId.Unit))
{
Creature creature = _sourceUnit.ToCreature();
if (creature.CanWalk())
includeFlags |= NavArea.Ground;
includeFlags |= NavTerrainFlag.Ground;
// creatures don't take environmental damage
if (creature.CanSwim())
includeFlags |= (NavArea.Water | NavArea.MagmaSlime);
includeFlags |= (NavTerrainFlag.Water | NavTerrainFlag.MagmaSlime);
}
else
includeFlags = (NavArea.Ground | NavArea.Water | NavArea.MagmaSlime);
includeFlags = (NavTerrainFlag.Ground | NavTerrainFlag.Water | NavTerrainFlag.MagmaSlime);
_filter.setIncludeFlags((ushort)includeFlags);
_filter.setExcludeFlags((ushort)excludeFlags);