mirror of
https://github.com/recastnavigation/recastnavigation.git
synced 2026-11-18 13:56:00 +00:00
- use "cylinder distance" for nearest point in polygon
- added option for findStraightPath() to append vertices at get edge crossings - added scale parameter for .obj loader
This commit is contained in:
@@ -534,9 +534,6 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
|
||||
const unsigned int ip = (unsigned int)(poly - tile->polys);
|
||||
const dtPolyDetail* pd = &tile->detailMeshes[ip];
|
||||
|
||||
// TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh.
|
||||
// Test and enable.
|
||||
/*
|
||||
// Clamp point to be inside the polygon.
|
||||
float verts[DT_VERTS_PER_POLYGON*3];
|
||||
float edged[DT_VERTS_PER_POLYGON];
|
||||
@@ -583,8 +580,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
|
||||
break;
|
||||
}
|
||||
}
|
||||
*/
|
||||
float closestDistSqr = FLT_MAX;
|
||||
|
||||
/* float closestDistSqr = FLT_MAX;
|
||||
for (int j = 0; j < pd->triCount; ++j)
|
||||
{
|
||||
const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
|
||||
@@ -606,7 +603,7 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
|
||||
dtVcopy(closest, pt);
|
||||
closestDistSqr = d;
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
/// @par
|
||||
@@ -1543,6 +1540,87 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
|
||||
return DT_SUCCESS | details;
|
||||
}
|
||||
|
||||
|
||||
dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
|
||||
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
|
||||
int* straightPathCount, const int maxStraightPath) const
|
||||
{
|
||||
if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
|
||||
{
|
||||
// The vertices are equal, update flags and poly.
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[(*straightPathCount)-1] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[(*straightPathCount)-1] = ref;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Append new vertex.
|
||||
dtVcopy(&straightPath[(*straightPathCount)*3], pos);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[(*straightPathCount)] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[(*straightPathCount)] = ref;
|
||||
(*straightPathCount)++;
|
||||
// If reached end of path or there is no space to append more vertices, return.
|
||||
if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath)
|
||||
{
|
||||
return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
}
|
||||
}
|
||||
return DT_IN_PROGRESS;
|
||||
}
|
||||
|
||||
dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
|
||||
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
|
||||
int* straightPathCount, const int maxStraightPath, const int options) const
|
||||
{
|
||||
const float* startPos = &straightPath[(*straightPathCount-1)*3];
|
||||
// Append or update last vertex
|
||||
dtStatus stat = 0;
|
||||
for (int i = startIdx; i < endIdx; i++)
|
||||
{
|
||||
// Calculate portal
|
||||
const dtPolyRef from = path[i];
|
||||
const dtMeshTile* fromTile = 0;
|
||||
const dtPoly* fromPoly = 0;
|
||||
if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
|
||||
const dtPolyRef to = path[i+1];
|
||||
const dtMeshTile* toTile = 0;
|
||||
const dtPoly* toPoly = 0;
|
||||
if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
|
||||
float left[3], right[3];
|
||||
if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
|
||||
break;
|
||||
|
||||
if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
|
||||
{
|
||||
// Skip intersection if only area crossings are requested.
|
||||
if (fromPoly->getArea() == toPoly->getArea())
|
||||
continue;
|
||||
}
|
||||
|
||||
// Append intersection
|
||||
float s,t;
|
||||
if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
|
||||
{
|
||||
float pt[3];
|
||||
dtVlerp(pt, left,right, t);
|
||||
|
||||
stat = appendVertex(pt, 0, path[i+1],
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
}
|
||||
}
|
||||
return DT_IN_PROGRESS;
|
||||
}
|
||||
|
||||
/// @par
|
||||
///
|
||||
/// This method peforms what is often called 'string pulling'.
|
||||
@@ -1563,7 +1641,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
|
||||
dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
|
||||
const dtPolyRef* path, const int pathSize,
|
||||
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
|
||||
int* straightPathCount, const int maxStraightPath) const
|
||||
int* straightPathCount, const int maxStraightPath, const int options) const
|
||||
{
|
||||
dtAssert(m_nav);
|
||||
|
||||
@@ -1575,30 +1653,24 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
if (!path[0])
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
|
||||
int n = 0;
|
||||
dtStatus stat = 0;
|
||||
|
||||
// TODO: Should this be callers responsibility?
|
||||
float closestStartPos[3];
|
||||
if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
|
||||
// Add start point.
|
||||
dtVcopy(&straightPath[n*3], closestStartPos);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n] = DT_STRAIGHTPATH_START;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n] = path[0];
|
||||
n++;
|
||||
if (n >= maxStraightPath)
|
||||
{
|
||||
*straightPathCount = n;
|
||||
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
|
||||
}
|
||||
|
||||
|
||||
float closestEndPos[3];
|
||||
if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
|
||||
// Add start point.
|
||||
stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
|
||||
if (pathSize > 1)
|
||||
{
|
||||
float portalApex[3], portalLeft[3], portalRight[3];
|
||||
@@ -1633,17 +1705,20 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
// This should only happen when the first polygon is invalid.
|
||||
return DT_FAILURE | DT_INVALID_PARAM;
|
||||
}
|
||||
|
||||
// Apeend portals along the current straight path segment.
|
||||
if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
|
||||
{
|
||||
stat = appendPortals(apexIndex, i, closestEndPos, path,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath, options);
|
||||
}
|
||||
|
||||
stat = appendVertex(closestEndPos, 0, path[i],
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
|
||||
dtVcopy(&straightPath[n*3], closestEndPos);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n] = 0;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n] = path[i];
|
||||
n++;
|
||||
|
||||
*straightPathCount = n;
|
||||
|
||||
return DT_SUCCESS | DT_PARTIAL_RESULT | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
}
|
||||
|
||||
// If starting really close the portal, advance.
|
||||
@@ -1675,6 +1750,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
}
|
||||
else
|
||||
{
|
||||
// Append portals along the current straight path segment.
|
||||
if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
|
||||
{
|
||||
stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath, options);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
}
|
||||
|
||||
dtVcopy(portalApex, portalLeft);
|
||||
apexIndex = leftIndex;
|
||||
|
||||
@@ -1685,30 +1770,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
|
||||
dtPolyRef ref = leftPolyRef;
|
||||
|
||||
if (!dtVequal(&straightPath[(n-1)*3], portalApex))
|
||||
{
|
||||
// Append new vertex.
|
||||
dtVcopy(&straightPath[n*3], portalApex);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n] = ref;
|
||||
n++;
|
||||
// If reached end of path or there is no space to append more vertices, return.
|
||||
if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
|
||||
{
|
||||
*straightPathCount = n;
|
||||
return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// The vertices are equal, update flags and poly.
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n-1] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n-1] = ref;
|
||||
}
|
||||
// Append or update vertex
|
||||
stat = appendVertex(portalApex, flags, ref,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
|
||||
dtVcopy(portalLeft, portalApex);
|
||||
dtVcopy(portalRight, portalApex);
|
||||
@@ -1734,6 +1801,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
}
|
||||
else
|
||||
{
|
||||
// Append portals along the current straight path segment.
|
||||
if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
|
||||
{
|
||||
stat = appendPortals(apexIndex, rightIndex, portalRight, path,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath, options);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
}
|
||||
|
||||
dtVcopy(portalApex, portalRight);
|
||||
apexIndex = rightIndex;
|
||||
|
||||
@@ -1743,31 +1820,13 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
|
||||
flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
|
||||
dtPolyRef ref = rightPolyRef;
|
||||
|
||||
if (!dtVequal(&straightPath[(n-1)*3], portalApex))
|
||||
{
|
||||
// Append new vertex.
|
||||
dtVcopy(&straightPath[n*3], portalApex);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n] = ref;
|
||||
n++;
|
||||
// If reached end of path or there is no space to append more vertices, return.
|
||||
if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
|
||||
{
|
||||
*straightPathCount = n;
|
||||
return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// The vertices are equal, update flags and poly.
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n-1] = flags;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n-1] = ref;
|
||||
}
|
||||
|
||||
// Append or update vertex
|
||||
stat = appendVertex(portalApex, flags, ref,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
|
||||
dtVcopy(portalLeft, portalApex);
|
||||
dtVcopy(portalRight, portalApex);
|
||||
@@ -1781,26 +1840,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Append portals along the current straight path segment.
|
||||
if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
|
||||
{
|
||||
stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath, options);
|
||||
if (stat != DT_IN_PROGRESS)
|
||||
return stat;
|
||||
}
|
||||
}
|
||||
|
||||
stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
|
||||
straightPath, straightPathFlags, straightPathRefs,
|
||||
straightPathCount, maxStraightPath);
|
||||
|
||||
// If the point already exists, remove it and add reappend the actual end location.
|
||||
if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos))
|
||||
n--;
|
||||
|
||||
// Add end point.
|
||||
if (n < maxStraightPath)
|
||||
{
|
||||
dtVcopy(&straightPath[n*3], closestEndPos);
|
||||
if (straightPathFlags)
|
||||
straightPathFlags[n] = DT_STRAIGHTPATH_END;
|
||||
if (straightPathRefs)
|
||||
straightPathRefs[n] = 0;
|
||||
n++;
|
||||
}
|
||||
|
||||
*straightPathCount = n;
|
||||
|
||||
return DT_SUCCESS | ((n >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
|
||||
}
|
||||
|
||||
/// @par
|
||||
|
||||
Reference in New Issue
Block a user