server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- use "cylinder distance" for nearest point in polygon

Browse Source 
- added option for findStraightPath() to append vertices at get edge crossings
- added scale parameter for .obj loader
This commit is contained in:
Mikko Mononen 2012-09-05 05:57:31 +00:00
parent a02cc64e4b
commit de6fd8c3ef
10 changed files with 271 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Detour/Include/DetourCommon.h
View File

@ -376,6 +376,10 @@ bool dtIntersectSegmentPoly2D(const float* p0, const float* p1,
float& tmin, float& tmax,
int& segMin, int& segMax);
bool dtIntersectSegSeg2D(const float* ap, const float* aq,
const float* bp, const float* bq,
float& s, float& t);
/// Determines if the specified point is inside the convex polygon on the xz-plane.
/// @param[in] pt The point to check. [(x, y, z)]
/// @param[in] verts The polygon vertices. [(x, y, z) * @p nverts]

7
Detour/Include/DetourNavMesh.h
View File

@ -87,6 +87,13 @@ enum dtStraightPathFlags
DT_STRAIGHTPATH_OFFMESH_CONNECTION = 0x04, ///< The vertex is the start of an off-mesh connection.
};
/// Options for dtNavMeshQuery::findStraightPath.
enum dtStraightPathOptions
{
DT_STRAIGHTPATH_AREA_CROSSINGS = 0x01, ///< Add a vertex at every polygon edge crossing where area changes.
DT_STRAIGHTPATH_ALL_CROSSINGS = 0x02, ///< Add a vertex at every polygon edge crossing.
};
/// Flags representing the type of a navigation mesh polygon.
enum dtPolyTypes
{

13
Detour/Include/DetourNavMeshQuery.h
View File

@ -158,11 +158,12 @@ public:
/// @param[out] straightPathRefs The reference id of the polygon that is being entered at each point. [opt]
/// @param[out] straightPathCount The number of points in the straight path.
/// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0]
/// @param[in] options Query options. (see: #dtStraightPathOptions)
/// @returns The status flags for the query.
dtStatus 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 = 0) const;
///@}
/// @name Sliced Pathfinding Functions
@ -446,6 +447,16 @@ private:
dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
float* mid) const;
// Appends vertex to a straight path
dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
int* straightPathCount, const int maxStraightPath) const;
// Appends intermediate portal points to a straight path.
dtStatus 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 dtNavMesh* m_nav; ///< Pointer to navmesh data.
struct dtQueryData

17
Detour/Source/DetourCommon.cpp
View File

@ -374,3 +374,20 @@ void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas,
out[2] = a*pa[2] + b*pb[2] + c*pc[2];
}
inline float vperpXZ(const float* a, const float* b) { return a[0]*b[2] - a[2]*b[0]; }
bool dtIntersectSegSeg2D(const float* ap, const float* aq,
const float* bp, const float* bq,
float& s, float& t)
{
float u[3], v[3], w[3];
dtVsub(u,aq,ap);
dtVsub(v,bq,bp);
dtVsub(w,ap,bp);
float d = vperpXZ(u,v);
if (fabsf(d) < 1e-6f) return false;
s = vperpXZ(v,w) / d;
t = vperpXZ(u,w) / d;
return true;
}

50
Detour/Source/DetourNavMesh.cpp
View File

@ -616,10 +616,48 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
const float* pos, float* closest) const
{
const dtPoly* poly = &tile->polys[ip];
// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
const float* v0 = &tile->verts[poly->verts[0]*3];
const float* v1 = &tile->verts[poly->verts[1]*3];
const float d0 = dtVdist(pos, v0);
const float d1 = dtVdist(pos, v1);
const float u = d0 / (d0+d1);
dtVlerp(closest, v0, v1, u);
return;
}
float closestDistSqr = FLT_MAX;
const dtPolyDetail* pd = &tile->detailMeshes[ip];
// Clamp point to be inside the polygon.
float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
float edget[DT_VERTS_PER_POLYGON];
const int nv = poly->vertCount;
for (int i = 0; i < nv; ++i)
dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
dtVcopy(closest, pos);
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
{
if (edged[i] < dmin)
{
dmin = edged[i];
imin = i;
}
}
const float* va = &verts[imin*3];
const float* vb = &verts[((imin+1)%nv)*3];
dtVlerp(closest, va, vb, edget[imin]);
}
// Find height at the location.
for (int j = 0; j < pd->triCount; ++j)
{
const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
@ -631,13 +669,11 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
else
v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
}
float pt[3];
dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
float d = dtVdistSqr(pos, pt);
if (d < closestDistSqr)
float h;
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
{
dtVcopy(closest, pt);
closestDistSqr = d;
closest[1] = h;
break;
}
}
}

254
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -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

2
RecastDemo/Include/MeshLoaderObj.h
View File

@ -40,7 +40,7 @@ private:
void addTriangle(int a, int b, int c, int& cap);
char m_filename[260];
float m_scale;
float* m_verts;
int* m_tris;
float* m_normals;

2
RecastDemo/Include/NavMeshTesterTool.h
View File

@ -47,6 +47,8 @@ class NavMeshTesterTool : public SampleTool
};
ToolMode m_toolMode;
int m_straightPathOptions;
static const int MAX_POLYS = 256;
static const int MAX_SMOOTH = 2048;

7
RecastDemo/Source/MeshLoaderObj.cpp
View File

@ -24,6 +24,7 @@
#include <math.h>
rcMeshLoaderObj::rcMeshLoaderObj() :
m_scale(1.0f),
m_verts(0),
m_tris(0),
m_normals(0),
@ -51,9 +52,9 @@ void rcMeshLoaderObj::addVertex(float x, float y, float z, int& cap)
m_verts = nv;
}
float* dst = &m_verts[m_vertCount*3];
*dst++ = x;
*dst++ = y;
*dst++ = z;
*dst++ = x*m_scale;
*dst++ = y*m_scale;
*dst++ = z*m_scale;
m_vertCount++;
}

28
RecastDemo/Source/NavMeshTesterTool.cpp
View File

@ -155,6 +155,7 @@ NavMeshTesterTool::NavMeshTesterTool() :
m_navQuery(0),
m_pathFindStatus(DT_FAILURE),
m_toolMode(TOOLMODE_PATHFIND_FOLLOW),
m_straightPathOptions(0),
m_startRef(0),
m_endRef(0),
m_npolys(0),
@ -218,6 +219,28 @@ void NavMeshTesterTool::handleMenu()
m_toolMode = TOOLMODE_PATHFIND_STRAIGHT;
recalc();
}
if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
{
imguiIndent();
imguiLabel("Vertices at crossings");
if (imguiCheck("None", m_straightPathOptions == 0))
{
m_straightPathOptions = 0;
recalc();
}
if (imguiCheck("Area", m_straightPathOptions == DT_STRAIGHTPATH_AREA_CROSSINGS))
{
m_straightPathOptions = DT_STRAIGHTPATH_AREA_CROSSINGS;
recalc();
}
if (imguiCheck("All", m_straightPathOptions == DT_STRAIGHTPATH_ALL_CROSSINGS))
{
m_straightPathOptions = DT_STRAIGHTPATH_ALL_CROSSINGS;
recalc();
}
imguiUnindent();
}
if (imguiCheck("Pathfind Sliced", m_toolMode == TOOLMODE_PATHFIND_SLICED))
{
m_toolMode = TOOLMODE_PATHFIND_SLICED;
@ -772,7 +795,7 @@ void NavMeshTesterTool::recalc()
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
m_straightPathPolys, &m_nstraightPath, MAX_POLYS, m_straightPathOptions);
}
}
else
@ -1033,7 +1056,8 @@ void NavMeshTesterTool::handleRender()
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT || m_toolMode == TOOLMODE_PATHFIND_SLICED)
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT ||
m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol);