- 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:
Mikko Mononen
2012-09-05 05:57:31 +00:00
parent a02cc64e4b
commit de6fd8c3ef
10 changed files with 271 additions and 113 deletions
+155 -99
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