Skip to content

Commit

Permalink
Merge remote-tracking branch 'remotes/recastnavigation/master' into t…
Browse files Browse the repository at this point in the history
…c_recast_demo
  • Loading branch information
jackpoz committed May 4, 2020
2 parents 8d66d9c + 54bb094 commit 2d211f5
Show file tree
Hide file tree
Showing 10 changed files with 8,323 additions and 214 deletions.
4 changes: 3 additions & 1 deletion DebugUtils/Source/DetourDebugDraw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,9 @@ static void drawPolyBoundaries(duDebugDraw* dd, const dtMeshTile* tile,
}
for (int m = 0, n = 2; m < 3; n=m++)
{
if (((t[3] >> (n*2)) & 0x3) == 0) continue; // Skip inner detail edges.
if ((dtGetDetailTriEdgeFlags(t[3], n) & DT_DETAIL_EDGE_BOUNDARY) == 0)
continue;

if (distancePtLine2d(tv[n],v0,v1) < thr &&
distancePtLine2d(tv[m],v0,v1) < thr)
{
Expand Down
22 changes: 22 additions & 0 deletions Detour/Include/DetourCommon.h
Original file line number Diff line number Diff line change
Expand Up @@ -283,6 +283,28 @@ inline bool dtVequal(const float* p0, const float* p1)
return d < thr;
}

/// Checks that the specified vector's components are all finite.
/// @param[in] v A point. [(x, y, z)]
/// @return True if all of the point's components are finite, i.e. not NaN
/// or any of the infinities.
inline bool dtVisfinite(const float* v)
{
bool result =
dtMathIsfinite(v[0]) &&
dtMathIsfinite(v[1]) &&
dtMathIsfinite(v[2]);

return result;
}

/// Checks that the specified vector's 2D components are finite.
/// @param[in] v A point. [(x, y, z)]
inline bool dtVisfinite2D(const float* v)
{
bool result = dtMathIsfinite(v[0]) && dtMathIsfinite(v[2]);
return result;
}

/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v)
/// @param[in] u A vector [(x, y, z)]
/// @param[in] v A vector [(x, y, z)]
Expand Down
4 changes: 4 additions & 0 deletions Detour/Include/DetourMath.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@ Members in this module are wrappers around the standard math library
#define DETOURMATH_H

#include <math.h>
// This include is required because libstdc++ has problems with isfinite
// if cmath is included before math.h.
#include <cmath>

inline float dtMathFabsf(float x) { return fabsf(x); }
inline float dtMathSqrtf(float x) { return sqrtf(x); }
Expand All @@ -16,5 +19,6 @@ inline float dtMathCeilf(float x) { return ceilf(x); }
inline float dtMathCosf(float x) { return cosf(x); }
inline float dtMathSinf(float x) { return sinf(x); }
inline float dtMathAtan2f(float y, float x) { return atan2f(y, x); }
inline bool dtMathIsfinite(float x) { return std::isfinite(x); }

#endif
21 changes: 20 additions & 1 deletion Detour/Include/DetourNavMesh.h
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,11 @@ enum dtRaycastOptions
DT_RAYCAST_USE_COSTS = 0x01, ///< Raycast should calculate movement cost along the ray and fill RaycastHit::cost
};

enum dtDetailTriEdgeFlags
{
DT_DETAIL_EDGE_BOUNDARY = 0x01, ///< Detail triangle edge is part of the poly boundary
};


/// Limit raycasting during any angle pahfinding
/// The limit is given as a multiple of the character radius
Expand Down Expand Up @@ -287,7 +292,8 @@ struct dtMeshTile
/// The detail mesh's unique vertices. [(x, y, z) * dtMeshHeader::detailVertCount]
float* detailVerts;

/// The detail mesh's triangles. [(vertA, vertB, vertC) * dtMeshHeader::detailTriCount]
/// The detail mesh's triangles. [(vertA, vertB, vertC, triFlags) * dtMeshHeader::detailTriCount].
/// See dtDetailTriEdgeFlags and dtGetDetailTriEdgeFlags.
unsigned char* detailTris;

/// The tile bounding volume nodes. [Size: dtMeshHeader::bvNodeCount]
Expand All @@ -305,6 +311,15 @@ struct dtMeshTile
dtMeshTile& operator=(const dtMeshTile&);
};

/// Get flags for edge in detail triangle.
/// @param triFlags[in] The flags for the triangle (last component of detail vertices above).
/// @param edgeIndex[in] The index of the first vertex of the edge. For instance, if 0,
/// returns flags for edge AB.
inline int dtGetDetailTriEdgeFlags(unsigned char triFlags, int edgeIndex)
{
return (triFlags >> (edgeIndex * 2)) & 0x3;
}

/// Configuration parameters used to define multi-tile navigation meshes.
/// The values are used to allocate space during the initialization of a navigation mesh.
/// @see dtNavMesh::init()
Expand Down Expand Up @@ -636,6 +651,8 @@ class dtNavMesh
/// Find nearest polygon within a tile.
dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
const float* halfExtents, float* nearestPt) const;
/// Returns whether position is over the poly and the height at the position if so.
bool getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const;
/// Returns closest point on polygon.
void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;

Expand All @@ -655,6 +672,8 @@ class dtNavMesh
unsigned int m_tileBits; ///< Number of tile bits in the tile ID.
unsigned int m_polyBits; ///< Number of poly bits in the tile ID.
#endif

friend class dtNavMeshQuery;
};

/// Allocates a navigation mesh object using the Detour allocator.
Expand Down
2 changes: 0 additions & 2 deletions Detour/Include/DetourNavMeshQuery.h
Original file line number Diff line number Diff line change
Expand Up @@ -119,8 +119,6 @@ class dtQueryFilter

};



/// Provides information about raycast hit
/// filled by dtNavMeshQuery::raycast
/// @ingroup detour
Expand Down
18 changes: 9 additions & 9 deletions Detour/Source/DetourCommon.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -203,14 +203,18 @@ void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const floa

bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h)
{
const float EPS = 1e-6f;
float v0[3], v1[3], v2[3];

dtVsub(v0, c,a);
dtVsub(v1, b,a);
dtVsub(v2, p,a);
dtVsub(v0, c, a);
dtVsub(v1, b, a);
dtVsub(v2, p, a);

// Compute scaled barycentric coordinates
float denom = v0[0] * v1[2] - v0[2] * v1[0];
if (fabsf(denom) < EPS)
return false;

float u = v1[2] * v2[0] - v1[0] * v2[2];
float v = v0[0] * v2[2] - v0[2] * v2[0];

Expand All @@ -220,13 +224,9 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b
v = -v;
}

// The (sloppy) epsilon is needed to allow to get height of points which
// are interpolated along the edges of the triangles.
float epsilon = - 1e-4f * denom;

// If point lies inside the triangle, return interpolated ycoord.
if (u >= epsilon && v >= epsilon && (u+v) <= denom - epsilon) {
h = a[1] + (v0[1]*u + v1[1]*v) / denom;
if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) {
h = a[1] + (v0[1] * u + v1[1] * v) / denom;
return true;
}
return false;
Expand Down
160 changes: 111 additions & 49 deletions Detour/Source/DetourNavMesh.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -616,63 +616,84 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
}
}

void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
namespace
{
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
getTileAndPolyByRefUnsafe(ref, &tile, &poly);

// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
template<bool onlyBoundary>
void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest)
{
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);
if (posOverPoly)
*posOverPoly = false;
return;
const unsigned int ip = (unsigned int)(poly - tile->polys);
const dtPolyDetail* pd = &tile->detailMeshes[ip];

float dmin = FLT_MAX;
float tmin = 0;
const float* pmin = 0;
const float* pmax = 0;

for (int i = 0; i < pd->triCount; i++)
{
const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4];
const int ANY_BOUNDARY_EDGE =
(DT_DETAIL_EDGE_BOUNDARY << 0) |
(DT_DETAIL_EDGE_BOUNDARY << 2) |
(DT_DETAIL_EDGE_BOUNDARY << 4);
if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0)
continue;

const float* v[3];
for (int j = 0; j < 3; ++j)
{
if (tris[j] < poly->vertCount)
v[j] = &tile->verts[poly->verts[tris[j]] * 3];
else
v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3];
}

for (int k = 0, j = 2; k < 3; j = k++)
{
if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 &&
(onlyBoundary || tris[j] < tris[k]))
{
// Only looking at boundary edges and this is internal, or
// this is an inner edge that we will see again or have already seen.
continue;
}

float t;
float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t);
if (d < dmin)
{
dmin = d;
tmin = t;
pmin = v[j];
pmax = v[k];
}
}
}

dtVlerp(closest, pmin, pmax, tmin);
}

}

bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const
{
// Off-mesh connections do not have detail polys and getting height
// over them does not make sense.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
return false;

const unsigned int ip = (unsigned int)(poly - tile->polys);
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 = edged[0];
int imin = 0;
for (int i = 1; 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]);

if (posOverPoly)
*posOverPoly = false;
}
else
{
if (posOverPoly)
*posOverPoly = true;
}
if (!dtPointInPolygon(pos, verts, nv))
return false;

if (!height)
return true;

// Find height at the location.
for (int j = 0; j < pd->triCount; ++j)
Expand All @@ -687,12 +708,53 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close
v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
}
float h;
if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h))
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
{
closest[1] = h;
break;
*height = h;
return true;
}
}

// If all triangle checks failed above (can happen with degenerate triangles
// or larger floating point values) the point is on an edge, so just select
// closest. This should almost never happen so the extra iteration here is
// ok.
float closest[3];
closestPointOnDetailEdges<false>(tile, poly, pos, closest);
*height = closest[1];
return true;
}

void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
getTileAndPolyByRefUnsafe(ref, &tile, &poly);

dtVcopy(closest, pos);
if (getPolyHeight(tile, poly, pos, &closest[1]))
{
if (posOverPoly)
*posOverPoly = true;
return;
}

if (posOverPoly)
*posOverPoly = false;

// 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];
float t;
dtDistancePtSegSqr2D(pos, v0, v1, t);
dtVlerp(closest, v0, v1, t);
return;
}

// Outside poly that is not an offmesh connection.
closestPointOnDetailEdges<true>(tile, poly, pos, closest);
}

dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
Expand Down
Loading

0 comments on commit 2d211f5

Please sign in to comment.