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

Slightly better heuristic for path finder (Mr. Ericsson would not approve!)

Browse Source 
Visualize A* open list.
Visualize parent nodes when querying nodes around.
This commit is contained in:
Mikko Mononen 2009-07-01 12:07:02 +00:00
parent b0cc29570e
commit 9edeccea35
8 changed files with 116 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Detour/Include/DetourStatNavMesh.h
View File

@ -148,7 +148,7 @@ public:
// Returns: Number of results.
int findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius,
dtPolyRef* resultRef, dtPolyRef* resultParent,
unsigned short* resultCost, unsigned short* resultDepth,
float* resultCost, unsigned short* resultDepth,
const int maxResult);
// Returns closest point on navigation polygon.
@ -159,9 +159,6 @@ public:
// Returns: true if closest point found.
bool closestPointToPoly(dtPolyRef ref, const float* pos, float* closest) const;
// Returns cost between two polygons.
unsigned short getCost(dtPolyRef from, dtPolyRef to) const;
// Returns pointer to a polygon based on ref.
const dtPoly* getPolyByRef(dtPolyRef ref) const;
// Returns number of navigation polygons.
@ -184,10 +181,15 @@ public:
private:
float getCost(dtPolyRef prev, dtPolyRef from, dtPolyRef to) const;
float getHeuristic(dtPolyRef from, dtPolyRef to) const;
bool getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const;
// Copies the locations of vertices of a polygon to an array.
int getPolyVerts(dtPolyRef ref, float* verts);
int getPolyVerts(dtPolyRef ref, float* verts) const;
// Returns portal points between two polygons.
bool getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right);
bool getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right) const;
unsigned char* m_data;
int m_dataSize;

7
Detour/Source/DetourDebugDraw.cpp
View File

@ -105,11 +105,16 @@ void dtDebugDrawStatNavMeshBVTree(const dtStatNavMesh* mesh)
void dtDebugDrawStatNavMesh(const dtStatNavMesh* mesh)
{
glColor4ub(0,196,255,64);
glBegin(GL_TRIANGLES);
for (int i = 0; i < mesh->getPolyCount(); ++i)
{
const dtPoly* p = mesh->getPoly(i);
if (mesh->isInOpenList(i+1))
glColor4ub(255,196,0,64);
else
glColor4ub(0,196,255,64);
unsigned short vi[3];
for (int j = 2; j < (int)p->nv; ++j)
{

79
Detour/Source/DetourStatNavMesh.cpp
View File

@ -326,8 +326,8 @@ struct dtNode
CLOSED = 0x02,
};
dtNode* parent;
unsigned short cost;
unsigned short total;
float cost;
float total;
unsigned short id;
unsigned short flags;
};
@ -606,17 +606,34 @@ bool dtStatNavMesh::init(unsigned char* data, int dataSize, bool ownsData)
return true;
}
unsigned short dtStatNavMesh::getCost(dtPolyRef from, dtPolyRef to) const
float dtStatNavMesh::getCost(dtPolyRef prev, dtPolyRef from, dtPolyRef to) const
{
const dtPoly* fromPoly = getPoly(prev ? prev-1 : from-1);
const dtPoly* toPoly = getPoly(to-1);
float fromPc[3], toPc[3];
calcPolyCenter(fromPc, fromPoly, m_verts);
calcPolyCenter(toPc, toPoly, m_verts);
float dx = fromPc[0]-toPc[0];
float dy = fromPc[1]-toPc[1];
float dz = fromPc[2]-toPc[2];
return sqrtf(dx*dx + dy*dy + dz*dz);
}
float dtStatNavMesh::getHeuristic(dtPolyRef from, dtPolyRef to) const
{
const dtPoly* fromPoly = getPoly(from-1);
const dtPoly* toPoly = getPoly(to-1);
float fromPc[3], toPc[3];
calcPolyCenter(fromPc, fromPoly, m_verts);
calcPolyCenter(toPc, toPoly, m_verts);
int cost = (int)(sqrtf(sqr(fromPc[0]-toPc[0]) + sqr(fromPc[2]-toPc[2])) * 4.0f);
if (cost < 1) cost = 1;
if (cost > 0xffff) cost = 0xffff;
return cost;
float dx = fromPc[0]-toPc[0];
float dy = fromPc[1]-toPc[1];
float dz = fromPc[2]-toPc[2];
return sqrtf(dx*dx + dy*dy + dz*dz) * 2.0f;
}
const dtPoly* dtStatNavMesh::getPolyByRef(dtPolyRef ref) const
@ -648,7 +665,7 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* startNode = m_nodePool->getNode(startRef);
startNode->parent = 0;
startNode->cost = 0;
startNode->total = getCost(startRef, endRef);
startNode->total = getHeuristic(startRef, endRef);
startNode->id = startRef;
startNode->flags = dtNode::OPEN;
m_openList->push(startNode);
@ -678,9 +695,9 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode newNode;
newNode.parent = bestNode;
newNode.id = neighbour;
newNode.cost = bestNode->cost + getCost(newNode.parent->id, newNode.id);
unsigned short costToGoal = getCost(newNode.id, endRef);
newNode.total = newNode.cost + costToGoal;
newNode.cost = bestNode->cost + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
float h = getHeuristic(newNode.id, endRef);
newNode.total = newNode.cost + h;
dtNode* actualNode = m_nodePool->getNode(newNode.id);
if (!actualNode)
@ -694,9 +711,9 @@ int dtStatNavMesh::findPath(dtPolyRef startRef, dtPolyRef endRef,
actualNode->cost = newNode.cost;
actualNode->total = newNode.total;
if (costToGoal < lastBestNodeCost)
if (h < lastBestNodeCost)
{
lastBestNodeCost = costToGoal;
lastBestNodeCost = h;
lastBestNode = actualNode;
}
@ -911,7 +928,7 @@ int dtStatNavMesh::findStraightPath(const float* startPos, const float* endPos,
return straightPathSize;
}
int dtStatNavMesh::getPolyVerts(dtPolyRef ref, float* verts)
int dtStatNavMesh::getPolyVerts(dtPolyRef ref, float* verts) const
{
if (!m_header) return 0;
const dtPoly* poly = getPolyByRef(ref);
@ -1061,7 +1078,7 @@ float dtStatNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* center
newNode.parent = bestNode;
newNode.id = neighbour;
newNode.cost = bestNode->cost + 1; // Depth
newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id);
newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
dtNode* actualNode = m_nodePool->getNode(newNode.id);
if (!actualNode)
@ -1097,7 +1114,7 @@ float dtStatNavMesh::findDistanceToWall(dtPolyRef centerRef, const float* center
int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos, float radius,
dtPolyRef* resultRef, dtPolyRef* resultParent,
unsigned short* resultCost, unsigned short* resultDepth,
float* resultCost, unsigned short* resultDepth,
const int maxResult)
{
if (!m_header) return 0;
@ -1158,7 +1175,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos,
newNode.parent = bestNode;
newNode.id = neighbour;
newNode.cost = bestNode->cost + 1; // Depth
newNode.total = bestNode->total + getCost(newNode.parent->id, newNode.id);
newNode.total = bestNode->total + getCost(newNode.parent->parent ? newNode.parent->parent->id : 0, newNode.parent->id, newNode.id);
dtNode* actualNode = m_nodePool->getNode(newNode.id);
if (!actualNode)
@ -1187,7 +1204,7 @@ int dtStatNavMesh::findPolysAround(dtPolyRef centerRef, const float* centerPos,
if (resultCost)
resultCost[n] = actualNode->total;
if (resultDepth)
resultDepth[n] = actualNode->cost;
resultDepth[n] = (unsigned short)actualNode->cost;
++n;
}
actualNode->flags = dtNode::OPEN;
@ -1284,7 +1301,31 @@ dtPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* exten
return nearest;
}
bool dtStatNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right)
bool dtStatNavMesh::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const
{
const dtPoly* fromPoly = getPolyByRef(from);
if (!fromPoly)
return false;
// Find common edge between the polygons and returns the segment end points.
for (unsigned i = 0, j = (int)fromPoly->nv - 1; i < (int)fromPoly->nv; j = i++)
{
unsigned short neighbour = fromPoly->n[j];
if (neighbour == to)
{
const float* left = getVertex(fromPoly->v[j]);
const float* right = getVertex(fromPoly->v[i]);
mid[0] = (left[0]+right[0])*0.5f;
mid[1] = (left[1]+right[1])*0.5f;
mid[2] = (left[2]+right[2])*0.5f;
return true;
}
}
return false;
}
bool dtStatNavMesh::getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right) const
{
const dtPoly* fromPoly = getPolyByRef(from);
if (!fromPoly)

1
Recast/Include/RecastDebugDraw.h
View File

@ -53,5 +53,6 @@ void rcDebugDrawCylinderWire(float minx, float miny, float minz, float maxx, flo
void rcDebugDrawBoxWire(float minx, float miny, float minz, float maxx, float maxy, float maxz, const float* col);
void rcDebugDrawBox(float minx, float miny, float minz, float maxx, float maxy, float maxz,
const float* col1, const float* col2);
void rcDrawArc(const float* p0, const float* p1);
#endif // RECAST_DEBUGDRAW_H

9
Recast/Source/RecastDebugDraw.cpp
View File

@ -413,6 +413,13 @@ static void drawArc(const float* p0, const float* p1)
}
}
void rcDrawArc(const float* p0, const float* p1)
{
glBegin(GL_LINES);
drawArc(p0, p1);
glEnd();
}
void rcDebugDrawRegionConnections(const rcContourSet& cset, const float* orig, float cs, float ch, const float alpha)
{
// Draw centers
@ -435,8 +442,6 @@ void rcDebugDrawRegionConnections(const rcContourSet& cset, const float* orig, f
{
getContourCenter(cont2, orig, cs, ch, pos2);
drawArc(pos, pos2);
// glVertex3fv(pos);
// glVertex3fv(pos2);
}
}
}

BIN
RecastDemo/Bin/Recast.app/Contents/MacOS/Recast
View File

Binary file not shown.

1
RecastDemo/Include/BuilderStatMesh.h
View File

@ -27,6 +27,7 @@ protected:
dtPolyRef m_startRef;
dtPolyRef m_endRef;
dtPolyRef m_polys[MAX_POLYS];
dtPolyRef m_parent[MAX_POLYS];
int m_npolys;
float m_straightPath[MAX_POLYS*3];
int m_nstraightPath;

34
RecastDemo/Source/BuilderStatMesh.cpp
View File

@ -159,11 +159,31 @@ void BuilderStatMesh::toolRecalc()
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
float dist = sqrtf(dx*dx + dz*dz);
m_npolys = m_navMesh->findPolysAround(m_startRef, m_spos, dist, m_polys, 0, 0, 0, MAX_POLYS);
m_npolys = m_navMesh->findPolysAround(m_startRef, m_spos, dist, m_polys, m_parent, 0, 0, MAX_POLYS);
}
}
}
static void getPolyCenter(dtStatNavMesh* navMesh, dtPolyRef ref, float* center)
{
const dtPoly* p = navMesh->getPolyByRef(ref);
if (!p) return;
center[0] = 0;
center[1] = 0;
center[2] = 0;
for (int i = 0; i < (int)p->nv; ++i)
{
const float* v = navMesh->getVertex(p->v[i]);
center[0] += v[0];
center[1] += v[1];
center[2] += v[2];
}
const float s = 1.0f / p->nv;
center[0] *= s;
center[1] *= s;
center[2] *= s;
}
void BuilderStatMesh::toolRender(int flags)
{
if (!m_navMesh)
@ -249,8 +269,20 @@ void BuilderStatMesh::toolRender(int flags)
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_AROUND)
{
glLineWidth(2.0f);
for (int i = 0; i < m_npolys; ++i)
{
dtDebugDrawStatNavMeshPoly(m_navMesh, m_polys[i], pathCol);
if (m_parent[i])
{
float p0[3], p1[3];
getPolyCenter(m_navMesh, m_polys[i], p0);
getPolyCenter(m_navMesh, m_parent[i], p1);
glColor4ub(0,0,0,128);
rcDrawArc(p0, p1);
}
}
glLineWidth(1.0f);
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];