Skip to content

Commit

Permalink
Merge pull request #218 from boxa/master
Browse files Browse the repository at this point in the history 
PathFinder: optimize calculated path points and fix points Z
  • Loading branch information
@boxa
boxa committed Feb 5, 2017
2 parents 8794ef0 + 0ee860d commit f0d9535
Show file tree
Hide file tree
Showing 5 changed files with 164 additions and 44 deletions.
165 changes: 131 additions & 34 deletions src/game/PathFinder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,10 @@
#include "Creature.h"
#include "PathFinder.h"
#include "Log.h"
#include "World.h"

#include "../recastnavigation/Detour/Include/DetourCommon.h"
#include "../recastnavigation/Detour/Include/DetourMath.h"

////////////////// PathFinder //////////////////
PathFinder::PathFinder(const Unit* owner) :
Expand Down Expand Up @@ -50,23 +52,29 @@ PathFinder::~PathFinder()

bool PathFinder::calculate(float destX, float destY, float destZ, bool forceDest)
{
// Vector3 oldDest = getEndPosition();
Vector3 dest(destX, destY, destZ);
setEndPosition(dest);
if (!MaNGOS::IsValidMapCoord(destX, destY, destZ))
return false;

float x, y, z;
m_sourceUnit->GetPosition(x, y, z);

if (!MaNGOS::IsValidMapCoord(x, y, z))
return false;

Vector3 start(x, y, z);
setStartPosition(start);

Vector3 dest(destX, destY, destZ);
setEndPosition(dest);

m_forceDestination = forceDest;

DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::calculate() for %u \n", m_sourceUnit->GetGUIDLow());

// make sure navMesh works - we can run on map w/o mmap
// check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
if (!m_navMesh || !m_navMeshQuery || m_sourceUnit->hasUnitState(UNIT_STAT_IGNORE_PATHFINDING) ||
!HaveTile(start) || !HaveTile(dest))
!HaveTile(start) || !HaveTile(dest))
{
BuildShortcut();
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
Expand All @@ -91,8 +99,7 @@ dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 po
for (uint32 i = 0; i < polyPathSize; ++i)
{
float closestPoint[VERTEX_SIZE];
dtStatus dtResult = m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint, nullptr);
if (dtStatusFailed(dtResult))
if (dtStatusFailed(m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint, nullptr)))
continue;

float d = dtVdist2DSqr(point, closestPoint);
Expand All @@ -108,7 +115,7 @@ dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 po
}

if (distance)
*distance = dtSqrt(minDist3d);
*distance = dtMathSqrtf(minDist3d);

return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
}
Expand Down Expand Up @@ -219,8 +226,7 @@ void PathFinder::BuildPolyPath(const Vector3& startPos, const Vector3& endPos)
{
float closestPoint[VERTEX_SIZE];
// we may want to use closestPointOnPolyBoundary instead
dtResult = m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, nullptr);
if (dtStatusSucceed(dtResult))
if (dtStatusSucceed(m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, nullptr)))
{
dtVcopy(endPoint, closestPoint);
setActualEndPosition(Vector3(endPoint[2], endPoint[0], endPoint[1]));
Expand Down Expand Up @@ -259,7 +265,14 @@ void PathFinder::BuildPolyPath(const Vector3& startPos, const Vector3& endPos)
for (pathStartIndex = 0; pathStartIndex < m_polyLength; ++pathStartIndex)
{
// here to catch few bugs
MANGOS_ASSERT(m_pathPolyRefs[pathStartIndex] != INVALID_POLYREF || m_sourceUnit->PrintEntryError("PathFinder::BuildPolyPath"));
if (m_pathPolyRefs[pathStartIndex] == INVALID_POLYREF)
{
sLog.outError("Invalid poly ref in BuildPolyPath. polyLength: %u, pathStartIndex: %u,"
" startPos: %s, endPos: %s, mapId: %u",
m_polyLength, pathStartIndex, startPos.toString().c_str(), endPos.toString().c_str(),
m_sourceUnit->GetMapId());
break;
}

if (m_pathPolyRefs[pathStartIndex] == startPoly)
{
Expand All @@ -269,11 +282,13 @@ void PathFinder::BuildPolyPath(const Vector3& startPos, const Vector3& endPos)
}

for (pathEndIndex = m_polyLength - 1; pathEndIndex > pathStartIndex; --pathEndIndex)
{
if (m_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
}

if (startPolyFound && endPolyFound)
Expand Down Expand Up @@ -310,15 +325,13 @@ void PathFinder::BuildPolyPath(const Vector3& startPos, const Vector3& endPos)

// we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
float suffixEndPoint[VERTEX_SIZE];
dtResult = m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, nullptr);
if (dtStatusFailed(dtResult))
if (dtStatusFailed(m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, nullptr)))
{
// we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
// try to recover by using prev polyref
--prefixPolyLength;
suffixStartPoly = m_pathPolyRefs[prefixPolyLength - 1];
dtResult = m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, nullptr);
if (dtStatusFailed(dtResult))
if (dtStatusFailed(m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, nullptr)))
{
// suffixStartPoly is still invalid, error state
BuildShortcut();
Expand Down Expand Up @@ -398,6 +411,7 @@ void PathFinder::BuildPointPath(const float* startPoint, const float* endPoint)
float pathPoints[MAX_POINT_PATH_LENGTH * VERTEX_SIZE];
uint32 pointCount = 0;
dtStatus dtResult;

if (m_useStraightPath)
{
dtResult = m_navMeshQuery->findStraightPath(
Expand Down Expand Up @@ -434,20 +448,85 @@ void PathFinder::BuildPointPath(const float* startPoint, const float* endPoint)
return;
}

m_pathPoints.resize(pointCount);
for (uint32 i = 0; i < pointCount; ++i)
m_pathPoints[i] = Vector3(pathPoints[i * VERTEX_SIZE + 2], pathPoints[i * VERTEX_SIZE], pathPoints[i * VERTEX_SIZE + 1]);
if (pointCount == m_pointPathLimit)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath FAILED! path sized %d returned, lower than limit set to %d\n", pointCount, m_pointPathLimit);
BuildShortcut();
m_type = PATHFIND_SHORT;
return;
}

if (pointCount > 2 && sWorld.getConfig(CONFIG_BOOL_PATH_FIND_OPTIMIZE))
{
uint32 tempPointCounter = 2;
uint8 cutLimit = 0;

PointsArray tempPathPoints;
tempPathPoints.resize(pointCount);

for (uint32 i = 0; i < pointCount; ++i) // y, z, x expected here
{
uint32 pointPos = i * VERTEX_SIZE;
tempPathPoints[i] = Vector3(pathPoints[pointPos + 2], pathPoints[pointPos], pathPoints[pointPos + 1]);
}

// Optimize points
Vector3 emptyVec = { 0.0f, 0.0f, 0.0f };

for (uint32 i = 1; i < pointCount - 1; ++i)
{
G3D::Vector3 p = tempPathPoints[i]; // Point
G3D::Vector3 p1 = tempPathPoints[i - 1]; // PrevPoint
G3D::Vector3 p2 = tempPathPoints[i + 1]; // NextPoint

float lineLen = (p1.y - p2.y) * p.x + (p2.x - p1.x) * p.y + (p1.x * p2.y - p2.x * p1.y);

if (fabs(lineLen) < LINE_FAULT && cutLimit < SKIP_POINT_LIMIT)
{
tempPathPoints[i] = emptyVec;
cutLimit++;
}
else
{
tempPointCounter++;
cutLimit = 0;
}
}

m_pathPoints.resize(tempPointCounter);

uint32 pointPos = 0;

for (uint32 i = 0; i < pointCount; ++i)
{
if (tempPathPoints[i] != emptyVec)
{
m_pathPoints[pointPos] = tempPathPoints[i];
pointPos++;
}
}

pointCount = tempPointCounter;
}
else
{
m_pathPoints.resize(pointCount);
for (uint32 i = 0; i < pointCount; ++i)
{
uint32 pointPos = i * VERTEX_SIZE;
m_pathPoints[i] = { pathPoints[pointPos + 2], pathPoints[pointPos], pathPoints[pointPos + 1] };
}
}

// first point is always our current location - we need the next one
setActualEndPosition(m_pathPoints[pointCount - 1]);

// force the given destination, if needed
if (m_forceDestination &&
(!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
(!(m_type & PATHFIND_NORMAL) || !inRange(getEndPosition(), getActualEndPosition(), 1.0f, 1.0f)))
{
// we may want to keep partial subpath
if (dist3DSqr(getActualEndPosition(), getEndPosition()) <
0.3f * dist3DSqr(getStartPosition(), getEndPosition()))
if (dist3DSqr(getActualEndPosition(), getEndPosition()) < 0.3f * dist3DSqr(getStartPosition(), getEndPosition()))
{
setActualEndPosition(getEndPosition());
m_pathPoints[m_pathPoints.size() - 1] = getEndPosition();
Expand All @@ -461,9 +540,20 @@ void PathFinder::BuildPointPath(const float* startPoint, const float* endPoint)
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
}

NormalizePath();

DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
}

void PathFinder::NormalizePath()
{
if (!sWorld.getConfig(CONFIG_BOOL_PATH_FIND_NORMALIZE_Z))
return;

for (uint32 i = 0; i < m_pathPoints.size(); ++i)
m_sourceUnit->UpdateAllowedPositionZ(m_pathPoints[i].x, m_pathPoints[i].y, m_pathPoints[i].z);
}

void PathFinder::BuildShortcut()
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathFinder::BuildShortcut :: making shortcut\n");
Expand All @@ -477,6 +567,8 @@ void PathFinder::BuildShortcut()
m_pathPoints[0] = getStartPosition();
m_pathPoints[1] = getActualEndPosition();

NormalizePath();

m_type = PATHFIND_SHORTCUT;
}

Expand Down Expand Up @@ -525,7 +617,8 @@ void PathFinder::updateFilter()
NavTerrain PathFinder::getNavTerrain(float x, float y, float z) const
{
GridMapLiquidData data;
m_sourceUnit->GetTerrain()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data);
if (m_sourceUnit->GetTerrain()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data) == LIQUID_MAP_NO_WATER)
return NAV_GROUND;

switch (data.type_flags)
{
Expand All @@ -543,15 +636,21 @@ NavTerrain PathFinder::getNavTerrain(float x, float y, float z) const

bool PathFinder::HaveTile(const Vector3& p) const
{
int tx, ty;
int tx = -1, ty = -1;
float point[VERTEX_SIZE] = {p.y, p.z, p.x};

m_navMesh->calcTileLoc(point, &tx, &ty);
return (m_navMesh->getTileAt(tx, ty, 0) != nullptr);

/// Workaround
/// For some reason, often the tx and ty variables wont get a valid value
/// Use this check to prevent getting negative tile coords and crashing on getTileAt
if (tx == -1 || ty == -1)
return false;

return (m_navMesh->getTileAt(tx, ty, 0) != nullptr); // Don't use layer so always set to 0
}

uint32 PathFinder::fixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath,
const dtPolyRef* visited, uint32 nvisited)
uint32 PathFinder::fixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath, dtPolyRef const* visited, uint32 nvisited)
{
int32 furthestPath = -1;
int32 furthestVisited = -1;
Expand Down Expand Up @@ -641,16 +740,15 @@ dtStatus PathFinder::findSmoothPath(const float* startPos, const float* endPos,
uint32 nsmoothPath = 0;

dtPolyRef polys[MAX_PATH_LENGTH];
memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
memcpy(polys, polyPath, polyPathSize * sizeof(dtPolyRef));
uint32 npolys = polyPathSize;

float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
dtStatus dtResult = m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos);
if (dtStatusFailed(dtResult))
float iterPos[VERTEX_SIZE];
if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos)))
return DT_FAILURE;

dtResult = m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos);
if (dtStatusFailed(dtResult))
float targetPos[VERTEX_SIZE];
if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos)))
return DT_FAILURE;

dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
Expand All @@ -674,7 +772,7 @@ dtStatus PathFinder::findSmoothPath(const float* startPos, const float* endPos,
// Find movement delta.
float delta[VERTEX_SIZE];
dtVsub(delta, steerPos, iterPos);
float len = dtSqrt(dtVdot(delta, delta));
float len = dtMathSqrtf(dtVdot(delta, delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < SMOOTH_PATH_STEP_SIZE)
len = 1.0f;
Expand Down Expand Up @@ -729,8 +827,7 @@ dtStatus PathFinder::findSmoothPath(const float* startPos, const float* endPos,

// Handle the connection.
float newStartPos[VERTEX_SIZE], newEndPos[VERTEX_SIZE];
dtResult = m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, newStartPos, newEndPos);
if (dtStatusSucceed(dtResult))
if (dtStatusSucceed(m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, newStartPos, newEndPos)))
{
if (nsmoothPath < maxSmoothPathSize)
{
Expand Down
8 changes: 7 additions & 1 deletion src/game/PathFinder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ class Unit;
#define SMOOTH_PATH_STEP_SIZE 4.0f
#define SMOOTH_PATH_SLOP 0.3f

// How many points can be cutted
// May occupt visual bugs when lenght > 20y
#define SKIP_POINT_LIMIT 6
#define LINE_FAULT 0.5f
#define VERTEX_SIZE 3
#define INVALID_POLYREF 0

Expand All @@ -49,7 +53,8 @@ enum PathType
PATHFIND_SHORTCUT = 0x0002, // travel through obstacles, terrain, air, etc (old behavior)
PATHFIND_INCOMPLETE = 0x0004, // we have partial path to follow - getting closer to target
PATHFIND_NOPATH = 0x0008, // no valid path at all or error in generating one
PATHFIND_NOT_USING_PATH = 0x0010 // used when we are either flying/swiming or on map w/o mmaps
PATHFIND_NOT_USING_PATH = 0x0010, // used when we are either flying/swiming or on map w/o mmaps
PATHFIND_SHORT = 0x0020, // path is longer or equal to its limited path length
};

class PathFinder
Expand Down Expand Up @@ -99,6 +104,7 @@ class PathFinder
void setStartPosition(const Vector3& point) { m_startPosition = point; }
void setEndPosition(const Vector3& point) { m_actualEndPosition = point; m_endPosition = point; }
void setActualEndPosition(const Vector3& point) { m_actualEndPosition = point; }
void NormalizePath();

void clear()
{
Expand Down
3 changes: 3 additions & 0 deletions src/game/World.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -870,6 +870,9 @@ void World::LoadConfigSettings(bool reload)
MMAP::MMapFactory::preventPathfindingOnMaps(ignoreMapIds.c_str());
sLog.outString("WORLD: MMap pathfinding %sabled", getConfig(CONFIG_BOOL_MMAP_ENABLED) ? "en" : "dis");

setConfig(CONFIG_BOOL_PATH_FIND_OPTIMIZE, "PathFinder.OptimizePath", true);
setConfig(CONFIG_BOOL_PATH_FIND_NORMALIZE_Z, "PathFinder.NormalizeZ", false);

sLog.outString();
}

Expand Down
2 changes: 2 additions & 0 deletions src/game/World.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,6 +351,8 @@ enum eConfigBoolValues
CONFIG_BOOL_PET_UNSUMMON_AT_MOUNT,
CONFIG_BOOL_MMAP_ENABLED,
CONFIG_BOOL_PLAYER_COMMANDS,
CONFIG_BOOL_PATH_FIND_OPTIMIZE,
CONFIG_BOOL_PATH_FIND_NORMALIZE_Z,
CONFIG_BOOL_VALUE_COUNT
};

Expand Down

4 comments on commit f0d9535

@killerwife
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Next time you need to merge PR without merge commit. You need to rebase the branch, either through github or manually through commandline. Also, port it to TBC and classic

@boxa
Copy link
Contributor Author

@boxa boxa commented on f0d9535 Feb 5, 2017 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@killerwife

Next time you need to merge PR without merge commit. You need to rebase the branch, either through github or manually through commandline.

this option? \/

image

i did PR in order to check the compilation on Travis. usually i do commits directly, and these problems are not seen.

Also, port it to TBC and classic

TBC and Vanilla pathfinder part compatible with WoTLK?
i don't use them, so i don't know. if so, there is no problem

@killerwife
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, and yes afaik pathfinder is compatible.

@boxa
Copy link
Contributor Author

@boxa boxa commented on f0d9535 Feb 5, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok. done

Please sign in to comment.