|
|
|
|
@ -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)
|
|
|
|
|
@ -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,
|
|
|
|
|
|
elsid
5 years ago