diff --git a/Detour/Include/DetourStatNavMesh.h b/Detour/Include/DetourStatNavMesh.h index be0b26d..062551f 100755 --- a/Detour/Include/DetourStatNavMesh.h +++ b/Detour/Include/DetourStatNavMesh.h @@ -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; diff --git a/Detour/Source/DetourDebugDraw.cpp b/Detour/Source/DetourDebugDraw.cpp index 029f697..59ee322 100755 --- a/Detour/Source/DetourDebugDraw.cpp +++ b/Detour/Source/DetourDebugDraw.cpp @@ -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) { diff --git a/Detour/Source/DetourStatNavMesh.cpp b/Detour/Source/DetourStatNavMesh.cpp index e251963..5d1e18c 100755 --- a/Detour/Source/DetourStatNavMesh.cpp +++ b/Detour/Source/DetourStatNavMesh.cpp @@ -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) diff --git a/Recast/Include/RecastDebugDraw.h b/Recast/Include/RecastDebugDraw.h index b90d0ec..f40064e 100644 --- a/Recast/Include/RecastDebugDraw.h +++ b/Recast/Include/RecastDebugDraw.h @@ -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 diff --git a/Recast/Source/RecastDebugDraw.cpp b/Recast/Source/RecastDebugDraw.cpp index bbc69ea..024b3c8 100644 --- a/Recast/Source/RecastDebugDraw.cpp +++ b/Recast/Source/RecastDebugDraw.cpp @@ -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); } } } diff --git a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast index a91d566..c4bed81 100755 Binary files a/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast and b/RecastDemo/Bin/Recast.app/Contents/MacOS/Recast differ diff --git a/RecastDemo/Include/BuilderStatMesh.h b/RecastDemo/Include/BuilderStatMesh.h index 232d9c6..7a743aa 100644 --- a/RecastDemo/Include/BuilderStatMesh.h +++ b/RecastDemo/Include/BuilderStatMesh.h @@ -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; diff --git a/RecastDemo/Source/BuilderStatMesh.cpp b/RecastDemo/Source/BuilderStatMesh.cpp index ca82b21..63cedc6 100644 --- a/RecastDemo/Source/BuilderStatMesh.cpp +++ b/RecastDemo/Source/BuilderStatMesh.cpp @@ -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];