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

Merge branch 'master' of https://github.com/axelrodR/recastnavigation

Browse Source
This commit is contained in:
axelrodR 2014-02-16 16:57:30 +02:00
commit 7bc913aada
10 changed files with 210 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Detour/Include/DetourNavMesh.h
View File

@ -613,8 +613,7 @@ private:
dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
const float* extents, float* nearestPt) const;
/// Returns closest point on polygon.
void closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
const float* pos, float* closest) const;
void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice.
float m_orig[3]; ///< Origin of the tile (0,0)

10
Detour/Include/DetourNavMeshQuery.h
View File

@ -381,8 +381,9 @@ public:
/// @param[in] ref The reference id of the polygon.
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point on the polygon. [(x, y, z)]
/// @param[out] posOverPoly True of the position is over the polygon.
/// @returns The status flags for the query.
dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const;
dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
/// Returns a point on the boundary closest to the source point if the source point is outside the
/// polygon's xz-bounds.
@ -431,12 +432,7 @@ private:
/// Queries polygons within a tile.
int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter,
dtPolyRef* polys, const int maxPolys) const;
/// Find nearest polygon within a tile.
dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
const dtQueryFilter* filter, float* nearestPt) const;
/// Returns closest point on polygon.
void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const;
/// Returns portal points between two polygons.
dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
unsigned char& fromType, unsigned char& toType) const;

44
Detour/Source/DetourNavMesh.cpp
View File

@ -617,10 +617,12 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
}
}
void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
const float* pos, float* closest) const
void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
const dtPoly* poly = &tile->polys[ip];
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)
{
@ -630,11 +632,14 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
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];
// Clamp point to be inside the polygon.
float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
@ -660,6 +665,14 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
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;
}
// Find height at the location.
@ -702,12 +715,27 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
{
dtPolyRef ref = polys[i];
float closestPtPoly[3];
closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly);
float d = dtVdistSqr(center, closestPtPoly);
float diff[3];
bool posOverPoly = false;
float d = 0;
closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
dtVsub(diff, center, closestPtPoly);
if (posOverPoly)
{
d = dtAbs(diff[1]) - tile->header->walkableClimb;
d = d > 0 ? d*d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < nearestDistanceSqr)
{
if (nearestPt)
dtVcopy(nearestPt, closestPtPoly);
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = ref;
}

107
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -501,7 +501,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
///
/// See closestPointOnPolyBoundary() for a limited but faster option.
///
dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
dtAssert(m_nav);
const dtMeshTile* tile = 0;
@ -511,14 +511,6 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
if (!tile)
return DT_FAILURE | DT_INVALID_PARAM;
closestPointOnPolyInTile(tile, poly, pos, closest);
return DT_SUCCESS;
}
void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
const float* pos, float* closest) const
{
// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
@ -528,7 +520,9 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
const float d1 = dtVdist(pos, v1);
const float u = d0 / (d0+d1);
dtVlerp(closest, v0, v1, u);
return;
if (posOverPoly)
*posOverPoly = false;
return DT_SUCCESS;
}
const unsigned int ip = (unsigned int)(poly - tile->polys);
@ -559,6 +553,14 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
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;
}
// Find height at the location.
@ -580,30 +582,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
break;
}
}
/* float closestDistSqr = FLT_MAX;
for (int j = 0; j < pd->triCount; ++j)
{
const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
const float* v[3];
for (int k = 0; k < 3; ++k)
{
if (t[k] < poly->vertCount)
v[k] = &tile->verts[poly->verts[t[k]]*3];
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)
{
dtVcopy(closest, pt);
closestDistSqr = d;
}
}*/
return DT_SUCCESS;
}
/// @par
@ -747,8 +727,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
{
dtPolyRef ref = polys[i];
float closestPtPoly[3];
closestPointOnPoly(ref, center, closestPtPoly);
float d = dtVdistSqr(center, closestPtPoly);
float diff[3];
bool posOverPoly = false;
float d = 0;
closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
dtVsub(diff, center, closestPtPoly);
if (posOverPoly)
{
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly);
d = dtAbs(diff[1]) - tile->header->walkableClimb;
d = d > 0 ? d*d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < nearestDistanceSqr)
{
if (nearestPt)
@ -764,42 +763,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
return DT_SUCCESS;
}
dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
const dtQueryFilter* filter, float* nearestPt) const
{
dtAssert(m_nav);
float bmin[3], bmax[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
// Get nearby polygons from proximity grid.
dtPolyRef polys[128];
int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = FLT_MAX;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef ref = polys[i];
const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
float closestPtPoly[3];
closestPointOnPolyInTile(tile, poly, center, closestPtPoly);
float d = dtVdistSqr(center, closestPtPoly);
if (d < nearestDistanceSqr)
{
if (nearestPt)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = ref;
}
}
return nearest;
}
int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
const dtQueryFilter* filter,
dtPolyRef* polys, const int maxPolys) const

6
DetourCrowd/Source/DetourCrowd.cpp
View File

@ -711,7 +711,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
if (reqPath[reqPathCount-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos);
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
if (dtStatusFailed(status))
reqPathCount = 0;
}
@ -855,7 +855,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
{
// Partial path, constrain target position inside the last polygon.
float nearest[3];
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
if (dtStatusSucceed(status))
dtVcopy(targetPos, nearest);
else
@ -949,7 +949,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
float nearest[3];
dtVcopy(nearest, agentPos);
agentRef = 0;
dtStatus status = m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
dtVcopy(agentPos, nearest);
if (!agentRef)

6
RecastDemo/Bin/Tests/raycast_test.txt Normal file
View File

@ -0,0 +1,6 @@
s Tile Mesh
f nav_test.obj
rc 45.133884 -0.533207 -3.775568 47.078232 7.797605 14.293253 0xffef 0x0
rc 52.979847 -2.778793 -2.914886 50.628868 -2.350212 13.917850 0xffef 0x0
rc 45.209217 2.024442 1.838851 46.888412 7.797606 15.772338 0xffef 0x0
rc 45.388317 -0.562073 -3.673226 46.651001 7.797606 15.513507 0xffef 0x0

2
RecastDemo/Include/TestCase.h
View File

@ -26,6 +26,7 @@ class TestCase
enum TestType
{
TEST_PATHFIND,
TEST_RAYCAST,
};
struct Test
@ -39,6 +40,7 @@ class TestCase
TestType type;
float spos[3], epos[3];
float nspos[3], nepos[3];
float radius;
int includeFlags, excludeFlags;
bool expand;

12
RecastDemo/Source/NavMeshTesterTool.cpp
View File

@ -506,8 +506,8 @@ void NavMeshTesterTool::handleToggle()
if (m_pathIterPolyCount)
{
// Iterate over the path to find smooth path on the detail mesh surface.
m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos);
m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos);
m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos, 0);
m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos, 0);
m_nsmoothPath = 0;
@ -650,7 +650,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/)
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys-1] != m_endRef)
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
@ -715,8 +715,8 @@ void NavMeshTesterTool::recalc()
int npolys = m_npolys;
float iterPos[3], targetPos[3];
m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos);
m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos);
m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos, 0);
m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos, 0);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
@ -855,7 +855,7 @@ void NavMeshTesterTool::recalc()
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys-1] != m_endRef)
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,

139
RecastDemo/Source/TestCase.cpp
View File

@ -141,6 +141,20 @@ bool TestCase::load(const char* filePath)
&test->epos[0], &test->epos[1], &test->epos[2],
&test->includeFlags, &test->excludeFlags);
}
else if (row[0] == 'r' && row[1] == 'c')
{
// Pathfind test.
Test* test = new Test;
memset(test, 0, sizeof(Test));
test->type = TEST_RAYCAST;
test->expand = false;
test->next = m_tests;
m_tests = test;
sscanf(row+2, "%f %f %f %f %f %f %x %x",
&test->spos[0], &test->spos[1], &test->spos[2],
&test->epos[0], &test->epos[1], &test->epos[2],
&test->includeFlags, &test->excludeFlags);
}
}
delete [] buf;
@ -187,8 +201,8 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
TimeVal findNearestPolyStart = getPerfTime();
dtPolyRef startRef, endRef;
navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, 0);
navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, 0);
navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, iter->nspos);
navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, iter->nepos);
TimeVal findNearestPolyEnd = getPerfTime();
iter->findNearestPolyTime += getPerfDeltaTimeUsec(findNearestPolyStart, findNearestPolyEnd);
@ -196,35 +210,82 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
if (!startRef || ! endRef)
continue;
// Find path
TimeVal findPathStart = getPerfTime();
if (iter->type == TEST_PATHFIND)
{
// Find path
TimeVal findPathStart = getPerfTime();
navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd);
// Find straight path
if (iter->npolys)
{
TimeVal findStraightPathStart = getPerfTime();
navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS);
navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys,
straight, 0, 0, &iter->nstraight, MAX_POLYS);
TimeVal findStraightPathEnd = getPerfTime();
iter->findStraightPathTime += getPerfDeltaTimeUsec(findStraightPathStart, findStraightPathEnd);
}
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd);
// Copy results
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
// Find straight path
if (iter->npolys)
{
TimeVal findStraightPathStart = getPerfTime();
navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys,
straight, 0, 0, &iter->nstraight, MAX_POLYS);
TimeVal findStraightPathEnd = getPerfTime();
iter->findStraightPathTime += getPerfDeltaTimeUsec(findStraightPathStart, findStraightPathEnd);
}
// Copy results
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
if (iter->nstraight)
{
iter->straight = new float[iter->nstraight*3];
memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight);
}
}
if (iter->nstraight)
else if (iter->type == TEST_RAYCAST)
{
iter->straight = new float[iter->nstraight*3];
memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight);
float t = 0;
float hitNormal[3], hitPos[3];
iter->straight = new float[2*3];
iter->nstraight = 2;
iter->straight[0] = iter->spos[0];
iter->straight[1] = iter->spos[1];
iter->straight[2] = iter->spos[2];
TimeVal findPathStart = getPerfTime();
navquery->raycast(startRef, iter->spos, iter->epos, &filter, &t, hitNormal, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd);
if (t > 1)
{
// No hit
dtVcopy(hitPos, iter->epos);
}
else
{
// Hit
dtVlerp(hitPos, iter->spos, iter->epos, t);
}
// Adjust height.
if (iter->npolys > 0)
{
float h = 0;
navquery->getPolyHeight(polys[iter->npolys-1], hitPos, &h);
hitPos[1] = h;
}
dtVcopy(&iter->straight[3], hitPos);
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
}
}
@ -259,6 +320,32 @@ void TestCase::handleRender()
glColor4ub(51,102,0,129);
glVertex3f(iter->epos[0],iter->epos[1]-0.3f,iter->epos[2]);
glVertex3f(iter->epos[0],iter->epos[1]+0.3f,iter->epos[2]);
if (iter->expand)
{
const float s = 0.1f;
glColor4ub(255,32,0,128);
glVertex3f(iter->spos[0]-s,iter->spos[1],iter->spos[2]);
glVertex3f(iter->spos[0]+s,iter->spos[1],iter->spos[2]);
glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]-s);
glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]+s);
glColor4ub(255,192,0,255);
glVertex3f(iter->nspos[0]-s,iter->nspos[1],iter->nspos[2]);
glVertex3f(iter->nspos[0]+s,iter->nspos[1],iter->nspos[2]);
glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]-s);
glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]+s);
glColor4ub(255,32,0,128);
glVertex3f(iter->epos[0]-s,iter->epos[1],iter->epos[2]);
glVertex3f(iter->epos[0]+s,iter->epos[1],iter->epos[2]);
glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]-s);
glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]+s);
glColor4ub(255,192,0,255);
glVertex3f(iter->nepos[0]-s,iter->nepos[1],iter->nepos[2]);
glVertex3f(iter->nepos[0]+s,iter->nepos[1],iter->nepos[2]);
glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]-s);
glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]+s);
}
if (iter->expand)
glColor4ub(255,192,0,255);

7
RecastDemo/Source/main.cpp
View File

@ -817,7 +817,6 @@ int main(int /*argc*/, char** /*argv*/)
{
delete geom;
geom = 0;
showLog = true;
logScroll = 0;
ctx.dumpLog("Geom load log %s:", meshName);
@ -827,6 +826,10 @@ int main(int /*argc*/, char** /*argv*/)
sample->handleMeshChanged(geom);
}
// This will ensure that tile & poly bits are updated in tiled sample.
if (sample)
sample->handleSettings();
ctx.resetLog();
if (sample && !sample->handleBuild())
{
@ -860,7 +863,7 @@ int main(int /*argc*/, char** /*argv*/)
}
rx = 45;
ry = -45;
glFogf(GL_FOG_START, camr*0.1f);
glFogf(GL_FOG_START, camr*0.2f);
glFogf(GL_FOG_END, camr*1.25f);
}