ElderShell/EverWrath
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 9
EverWrath/src/game/Movement/MovementGenerators/PathGenerator.cpp
ShinDarth 0aba3ae267 Corrected more copyrights
2016-08-19 23:24:44 +02:00

1011 lines
37 KiB
C++
Raw Blame History

/*
* Copyright (C) 2016+ AzerothCore <www.azerothcore.org>, released under GNU GPL v2 license: http://github.com/azerothcore/azerothcore-wotlk/LICENSE-GPL2
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "PathGenerator.h"
#include "Map.h"
#include "Creature.h"
#include "Log.h"
#include "CellImpl.h"
#include "Cell.h"
#include "DetourCommon.h"
#include "DetourNavMeshQuery.h"
////////////////// PathGenerator //////////////////
PathGenerator::PathGenerator(const Unit* owner) :
_polyLength(0), _type(PATHFIND_BLANK), _useStraightPath(false),
_forceDestination(false), _pointPathLimit(MAX_POINT_PATH_LENGTH),
_endPosition(G3D::Vector3::zero()), _sourceUnit(owner), _navMesh(NULL),
_navMeshQuery(NULL)
{
memset(_pathPolyRefs, 0, sizeof(_pathPolyRefs));
uint32 mapId = _sourceUnit->GetMapId();
//if (MMAP::MMapFactory::IsPathfindingEnabled(_sourceUnit->FindMap())) // pussywizard: checked before creating new PathGenerator
{
MMAP::MMapManager* mmap = MMAP::MMapFactory::createOrGetMMapManager();
TRINITY_READ_GUARD(ACE_RW_Thread_Mutex, mmap->GetManagerLock());
_navMesh = mmap->GetNavMesh(mapId);
_navMeshQuery = mmap->GetNavMeshQuery(mapId, _sourceUnit->GetInstanceId());
}
CreateFilter();
}
PathGenerator::~PathGenerator()
{
}
bool PathGenerator::CalculatePath(float destX, float destY, float destZ, bool forceDest)
{
float x, y, z;
if (!_sourceUnit->movespline->Finalized() && _sourceUnit->movespline->Initialized())
{
Movement::Location realpos = _sourceUnit->movespline->ComputePosition();
x = realpos.x; y = realpos.y; z = realpos.z;
}
else
_sourceUnit->GetPosition(x, y, z);
if (!Trinity::IsValidMapCoord(destX, destY, destZ) || !Trinity::IsValidMapCoord(x, y, z))
return false;
G3D::Vector3 dest(destX, destY, destZ);
SetEndPosition(dest);
G3D::Vector3 start(x, y, z);
SetStartPosition(start);
_forceDestination = forceDest;
// pussywizard: EnsureGridCreated may need map mutex, and it loads mmaps (may need mmap mutex)
// pussywizard: a deadlock can occur if here the map mutex is requested after acquiring mmap lock below
// pussywizard: so call EnsureGridCreated for all possible grids before acquiring mmap lock :/ this is so shit... because the core is shit :/
{
Cell cellS(start.x, start.y);
_sourceUnit->GetMap()->EnsureGridCreated(GridCoord(cellS.GridX(), cellS.GridY()));
Cell cellD(dest.x, dest.y);
_sourceUnit->GetMap()->EnsureGridCreated(GridCoord(cellD.GridX(), cellD.GridY()));
}
UpdateFilter(); // no mmap operations inside, no mutex needed
// pussywizard: mutex with new that can be release at any moment, DON'T FORGET TO RELEASE ON EVERY RETURN !!!
const Map* base = _sourceUnit->GetBaseMap();
ACE_RW_Thread_Mutex& mmapLock = (base ? base->GetMMapLock() : MMAP::MMapFactory::createOrGetMMapManager()->GetMMapGeneralLock());
mmapLock.acquire_read();
// 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 (!_navMesh || !_navMeshQuery || _sourceUnit->HasUnitState(UNIT_STATE_IGNORE_PATHFINDING) ||
_sourceUnit->GetObjectSize() >= SIZE_OF_GRIDS/2.0f || _sourceUnit->GetExactDistSq(destX, destY, destZ) >= (SIZE_OF_GRIDS*SIZE_OF_GRIDS/4.0f) ||
!HaveTile(start) || !HaveTile(dest))
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
mmapLock.release();
return true;
}
BuildPolyPath(start, dest, mmapLock);
return true;
}
dtPolyRef PathGenerator::GetPathPolyByPosition(dtPolyRef const* polyPath, uint32 polyPathSize, float const* point, float* distance) const
{
if (!polyPath || !polyPathSize)
return INVALID_POLYREF;
float polyHeight, height;
for (uint32 i = 0; i < polyPathSize; ++i)
{
if (DT_SUCCESS != _navMeshQuery->getPolyHeight(polyPath[i], point, &polyHeight))
continue;
height = point[1]-polyHeight;
if (height > 0.0f && height < ALLOWED_DIST_FROM_POLY+ADDED_Z_FOR_POLY_LOOKUP)
{
if (distance)
*distance = height;
return polyPath[i];
}
}
return INVALID_POLYREF;
}
dtPolyRef PathGenerator::GetPolyByLocation(float* point, float* distance) const
{
// first we check the current path
// if the current path doesn't contain the current poly,
// we need to use the expensive navMesh.findNearestPoly
point[1] += ADDED_Z_FOR_POLY_LOOKUP;
dtPolyRef polyRef = GetPathPolyByPosition(_pathPolyRefs, _polyLength, point, distance);
point[1] -= ADDED_Z_FOR_POLY_LOOKUP;
if (polyRef != INVALID_POLYREF)
return polyRef;
// we don't have it in our old path
// try to get it by findNearestPoly()
// first try with low search box
float extents[VERTEX_SIZE] = {3.0f, 5.0f, 3.0f}; // bounds of poly search area
float closestPoint[VERTEX_SIZE] = {0.0f, 0.0f, 0.0f};
dtStatus result = _navMeshQuery->findNearestPoly(point, extents, &_filter, &polyRef, closestPoint);
if (DT_SUCCESS == result && polyRef != INVALID_POLYREF)
{
*distance = dtVdist(closestPoint, point);
return polyRef;
}
// still nothing ..
// try with bigger search box
extents[1] = 80.0f;
result = _navMeshQuery->findNearestPoly(point, extents, &_filter, &polyRef, closestPoint);
if (DT_SUCCESS == result && polyRef != INVALID_POLYREF)
{
*distance = dtVdist(closestPoint, point);
return polyRef;
}
return INVALID_POLYREF;
}
G3D::Vector3 ClosestPointOnLine(const G3D::Vector3 & a, const G3D::Vector3 & b, const G3D::Vector3 & Point)
{
G3D::Vector3 c = Point - a; // Vector from a to Point
G3D::Vector3 v = (b - a).unit(); // Unit Vector from a to b
float d = (b - a).length(); // Length of the line segment
float t = v.dot(c); // Intersection point Distance from a
// Check to see if the point is on the line
// if not then return the endpoint
if(t < 0) return a;
if(t > d) return b;
// get the distance to move from point a
v *= t;
// move from point a to the nearest point on the segment
return a + v;
}
template <class MUTEX_TYPE>
class MutexReleaser
{
public:
MutexReleaser(MUTEX_TYPE& mutex) : _mutex(mutex) {}
~MutexReleaser() { _mutex.release(); }
private:
MUTEX_TYPE& _mutex;
};
void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 const& endPos, ACE_RW_Thread_Mutex& mmapLock)
{
bool endInWaterFar = false;
bool cutToFirstHigher = false;
{
MutexReleaser<ACE_RW_Thread_Mutex> mutexReleaser(mmapLock);
// *** getting start/end poly logic ***
float distToStartPoly, distToEndPoly;
float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
dtPolyRef startPoly = GetPolyByLocation(startPoint, &distToStartPoly);
dtPolyRef endPoly = GetPolyByLocation(endPoint, &distToEndPoly);
bool sourceIsFlying = (_sourceUnit->GetUnitMovementFlags() & (MOVEMENTFLAG_CAN_FLY|MOVEMENTFLAG_FLYING)) || (_sourceUnit->HasUnitMovementFlag(MOVEMENTFLAG_DISABLE_GRAVITY) && !_sourceUnit->HasUnitMovementFlag(MOVEMENTFLAG_SWIMMING)) || (_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)_sourceUnit)->CanFly());
bool sourceCanSwim = _sourceUnit->GetTypeId() == TYPEID_UNIT ? _sourceUnit->ToCreature()->CanSwim() : true;
bool sourceCanWalk = _sourceUnit->GetTypeId() == TYPEID_UNIT ? _sourceUnit->ToCreature()->CanWalk() : true;
// we have a hole in our mesh
// make shortcut path and mark it as NOPATH ( with flying and swimming exception )
// its up to caller how he will use this info
if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
BuildShortcut();
if (sourceIsFlying)
{
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
if (sourceCanSwim)
{
if ((startPoly == INVALID_POLYREF && LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(startPos.x, startPos.y, startPos.z, MAP_ALL_LIQUIDS, NULL)) ||
(endPoly == INVALID_POLYREF && LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(endPos.x, endPos.y, endPos.z, MAP_ALL_LIQUIDS, NULL)))
{
_type = PATHFIND_NOPATH;
return;
}
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
_type = PATHFIND_NOPATH;
return;
}
// we may need a better number here
bool farFromStartPoly = (distToStartPoly > ALLOWED_DIST_FROM_POLY);
bool farFromEndPoly = (distToEndPoly > ALLOWED_DIST_FROM_POLY);
if (farFromStartPoly)
{
if (sourceIsFlying)
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
if (sourceCanSwim)
{
if (LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(startPos.x, startPos.y, startPos.z, MAP_ALL_LIQUIDS, NULL))
{
if (distToStartPoly > MAX_FIXABLE_Z_ERROR)
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
if (farFromEndPoly)
{
if (LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(endPos.x, endPos.y, endPos.z, MAP_ALL_LIQUIDS, NULL))
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
}
else if (LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(endPos.x, endPos.y, endPos.z, MAP_ALL_LIQUIDS, NULL))
{
if (farFromEndPoly)
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
cutToFirstHigher = true;
}
else // starting and ending points are in water
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
}
else
{
if (distToStartPoly > MAX_FIXABLE_Z_ERROR || farFromEndPoly)
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
}
else if (farFromEndPoly)
{
if (sourceIsFlying)
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
if (LIQUID_MAP_NO_WATER == _sourceUnit->GetBaseMap()->getLiquidStatus(endPos.x, endPos.y, endPos.z, MAP_ALL_LIQUIDS, NULL))
{
if (!sourceCanWalk)
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
else
{
if (!sourceCanSwim)
{
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
// if both points are in water
if (LIQUID_MAP_NO_WATER != _sourceUnit->GetBaseMap()->getLiquidStatus(startPos.x, startPos.y, startPos.z, MAP_ALL_LIQUIDS, NULL))
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
endInWaterFar = true;
}
if (startPoly != endPoly || !endInWaterFar)
{
float closestPoint[VERTEX_SIZE];
if (DT_SUCCESS == _navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint))
{
dtVcopy(endPoint, closestPoint);
SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
}
_type = PATHFIND_INCOMPLETE;
}
}
// *** poly path generating logic ***
if (startPoly == endPoly)
{
BuildShortcut();
_type = !farFromEndPoly || endInWaterFar ? PATHFIND_NORMAL : PATHFIND_INCOMPLETE;
_pathPolyRefs[0] = startPoly;
_polyLength = 1;
return;
}
// look for startPoly/endPoly in current path
/// @todo we can merge it with getPathPolyByPosition() loop
bool startPolyFound = false;
bool endPolyFound = false;
uint32 pathStartIndex = 0;
uint32 pathEndIndex = 0;
if (_polyLength)
{
for (; pathStartIndex < _polyLength; ++pathStartIndex)
{
// here to catch few bugs
ASSERT(_pathPolyRefs[pathStartIndex] != INVALID_POLYREF);
if (_pathPolyRefs[pathStartIndex] == startPoly)
{
startPolyFound = true;
break;
}
}
for (pathEndIndex = _polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
if (_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
if (startPolyFound && endPolyFound)
{
_polyLength = pathEndIndex - pathStartIndex + 1;
memmove(_pathPolyRefs, _pathPolyRefs + pathStartIndex, _polyLength * sizeof(dtPolyRef));
}
else if (startPolyFound && !endPolyFound && _polyLength-pathStartIndex >= 3 /*if (>=3) then 70% will return at least one more than just startPoly*/)
{
// we are moving on the old path but target moved out
// so we have atleast part of poly-path ready
_polyLength -= pathStartIndex;
// try to adjust the suffix of the path instead of recalculating entire length
// at given interval the target cannot get too far from its last location
// thus we have less poly to cover
// sub-path of optimal path is optimal
// take ~65% of the original length
/// @todo play with the values here
uint32 prefixPolyLength = uint32(_polyLength * 0.7f + 0.5f); // this should be always >= 1
memmove(_pathPolyRefs, _pathPolyRefs+pathStartIndex, prefixPolyLength * sizeof(dtPolyRef));
dtPolyRef suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
bool error = false; // can't use a part of old path, generate whole new
// 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];
if (DT_SUCCESS != _navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
{
// we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
// try to recover by using prev polyref
--prefixPolyLength;
if (prefixPolyLength)
{
suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
if (DT_SUCCESS != _navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
error = true;
}
else
error = true;
}
if (!error)
{
// generate suffix
uint32 suffixPolyLength = 0;
dtStatus dtResult = _navMeshQuery->findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs + prefixPolyLength - 1, // [out] path
(int*)&suffixPolyLength,
MAX_PATH_LENGTH-prefixPolyLength); // max number of polygons in output path
if (!suffixPolyLength || dtResult != DT_SUCCESS)
{
// this is probably an error state, but we'll leave it
// and hopefully recover on the next Update
// we still need to copy our preffix
}
// new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
else
{
// free and invalidate old path data
Clear();
dtStatus dtResult = _navMeshQuery->findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs, // [out] path
(int*)&_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
if (!_polyLength || dtResult != DT_SUCCESS)
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
}
else
{
// either we have no path at all -> first run
// or something went really wrong -> we aren't moving along the path to the target
// pussywizard: or knocked back away from our path, nothing special
// just generate new path
// free and invalidate old path data
Clear();
dtStatus dtResult = _navMeshQuery->findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs, // [out] path
(int*)&_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
if (!_polyLength || dtResult != DT_SUCCESS)
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
// by now we know what type of path we can get
if (_pathPolyRefs[_polyLength - 1] == endPoly && !(_type & PATHFIND_INCOMPLETE))
_type = PATHFIND_NORMAL;
else
_type = PATHFIND_INCOMPLETE;
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
// pussywizard: no mmap usage below, release mutex
} // end of scope (mutex released in object destructor)
if (_type == PATHFIND_NORMAL && cutToFirstHigher) // starting in water, far from bottom, target is on the ground (above starting Z) -> update beginning points that are lower than starting Z
{
uint32 i = 0;
uint32 size = _pathPoints.size();
for (; i < size; ++i)
if (_pathPoints[i].z >= _sourceUnit->GetPositionZ()+0.1f)
break;
if (i && i != size && LIQUID_MAP_NO_WATER != _sourceUnit->GetBaseMap()->getLiquidStatus(_pathPoints[i-1].x, _pathPoints[i-1].y, _pathPoints[i-1].z, MAP_ALL_LIQUIDS, NULL))
for (uint32 j=0; j<i; ++j)
_pathPoints[j].z = _sourceUnit->GetPositionZ();
}
if (!_forceDestination)
if (uint32 lastIdx = _pathPoints.size())
{
lastIdx = lastIdx-1;
if (endInWaterFar)
{
SetActualEndPosition(GetEndPosition());
_pathPoints[lastIdx] = GetEndPosition();
}
else
_sourceUnit->UpdateAllowedPositionZ(_pathPoints[lastIdx].x, _pathPoints[lastIdx].y, _pathPoints[lastIdx].z);
}
// pussywizard: fix for running back and forth while target moves
// pussywizard: second point (first is actual position) is forward to current server position, but when received by the client it's already behind, so the npc runs back to that point
// pussywizard: the higher speed, the more probable the situation is, so try to move second point as far forward the path as possible
// pussywizard: changed path cannot differ much from the original (by max dist), because there might be walls and holes
if (_sourceUnit->GetCreatureType() != CREATURE_TYPE_NON_COMBAT_PET)
return;
uint32 size = _pathPoints.size();
bool ok = true;
for (uint32 i = 2; i <= size; ++i)
{
// pussywizard: line between start point and i'th point
// pussywizard: distance to that line of all points between 0 and i must be less than X and less than sourceUnit size
if (i < size)
{
ok = true;
if ((_pathPoints[i] - _pathPoints[0]).squaredLength() > 15.0f*15.0f)
ok = false;
else
for (uint32 j = 1; j < i; ++j)
{
float sqDist = (_pathPoints[j] - ClosestPointOnLine(_pathPoints[0], _pathPoints[i], _pathPoints[j])).squaredLength();
float oSize = _sourceUnit->GetObjectSize();
if (sqDist > 1.0f*1.0f || sqDist > oSize*oSize)
{
ok = false;
break;
}
}
}
if (!ok)
{
if (i < size)
{
// pussywizard: check additional 3 quarter points after last fitting poly point
G3D::Vector3 dir = _pathPoints[i] - _pathPoints[i-1];
G3D::Vector3 increment = (dir.length()/4.0f) * dir.unit();
for (uint8 k = 3; k > 0; --k)
{
G3D::Vector3 newPoint = _pathPoints[i-1] + ((float)k)*increment;
bool ok2 = true;
if ((newPoint - _pathPoints[0]).squaredLength() > 15.0f*15.0f)
ok2 = false;
else
for (uint32 j = 1; j < i; ++j)
{
float sqDist = (_pathPoints[j] - ClosestPointOnLine(_pathPoints[0], newPoint, _pathPoints[j])).squaredLength();
float oSize = _sourceUnit->GetObjectSize();
if (sqDist > 1.0f*1.0f || sqDist > oSize*oSize)
{
ok2 = false;
break;
}
}
if (ok2)
{
_pathPoints[i-1] = newPoint;
break;
}
}
// pussywizard: memmove crashes o_O
// memmove(&_pathPoints + sizeof(G3D::Vector3), &_pathPoints + (i-1)*sizeof(G3D::Vector3), (size-i+1)*sizeof(G3D::Vector3));
for (uint8 k = 1; k <= size-i+1; ++k)
_pathPoints[k] = _pathPoints[k+i-2];
_pathPoints.resize(size-i+2);
}
else if (size > 2)
{
_pathPoints[1] = _pathPoints[size-1];
_pathPoints.resize(2);
}
break;
}
}
}
void PathGenerator::BuildPointPath(const float *startPoint, const float *endPoint)
{
float pathPoints[MAX_POINT_PATH_LENGTH*VERTEX_SIZE];
uint32 pointCount = 0;
dtStatus dtResult = DT_FAILURE;
if (_useStraightPath)
{
dtResult = _navMeshQuery->findStraightPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs, // current path
_polyLength, // lenth of current path
pathPoints, // [out] path corner points
NULL, // [out] flags
NULL, // [out] shortened path
(int*)&pointCount,
_pointPathLimit); // maximum number of points/polygons to use
}
else
{
dtResult = FindSmoothPath(
startPoint, // start position
endPoint, // end position
_pathPolyRefs, // current path
_polyLength, // length of current path
pathPoints, // [out] path corner points
(int*)&pointCount,
_pointPathLimit); // maximum number of points
}
if (pointCount < 2 || dtResult != DT_SUCCESS)
{
// only happens if pass bad data to findStraightPath or navmesh is broken
// single point paths can be generated here
/// @todo check the exact cases
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
else if (pointCount == _pointPathLimit)
{
BuildShortcut();
_type = PATHFIND_SHORT;
return;
}
_pathPoints.resize(pointCount);
for (uint32 i = 0; i < pointCount; ++i)
_pathPoints[i] = G3D::Vector3(pathPoints[i*VERTEX_SIZE+2], pathPoints[i*VERTEX_SIZE], pathPoints[i*VERTEX_SIZE+1]);
// first point is always our current location - we need the next one
SetActualEndPosition(_pathPoints[pointCount-1]);
if (_forceDestination && (!(_type & PATHFIND_NORMAL) || !InRange(GetEndPosition(), GetActualEndPosition(), 0.75f, 0.75f)))
{
// we may want to keep partial subpath
if (Dist3DSqr(GetActualEndPosition(), GetEndPosition()) < 0.33f * Dist3DSqr(GetStartPosition(), GetEndPosition()))
{
SetActualEndPosition(GetEndPosition());
_pathPoints[_pathPoints.size()-1] = GetEndPosition();
}
else
{
SetActualEndPosition(GetEndPosition());
BuildShortcut();
}
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
}
}
void PathGenerator::BuildShortcut()
{
Clear();
_pathPoints.resize(2);
_pathPoints[0] = GetStartPosition();
_pathPoints[1] = GetActualEndPosition();
_type = PATHFIND_SHORTCUT;
}
void PathGenerator::CreateFilter()
{
uint16 includeFlags = 0;
uint16 excludeFlags = 0;
if (_sourceUnit->GetTypeId() == TYPEID_UNIT)
{
Creature* creature = (Creature*)_sourceUnit;
if (creature->CanWalk())
includeFlags |= NAV_GROUND; // walk
// creatures don't take environmental damage
if (creature->CanSwim())
includeFlags |= (NAV_WATER | NAV_MAGMA | NAV_SLIME); // swim
}
else // assume Player
{
// perfect support not possible, just stay 'safe'
includeFlags |= (NAV_GROUND | NAV_WATER | NAV_MAGMA | NAV_SLIME);
}
_filter.setIncludeFlags(includeFlags);
_filter.setExcludeFlags(excludeFlags);
UpdateFilter();
}
void PathGenerator::UpdateFilter()
{
// allow creatures to cheat and use different movement types if they are moved
// forcefully into terrain they can't normally move in
if (_sourceUnit->IsInWater() || _sourceUnit->IsUnderWater())
{
uint16 includedFlags = _filter.getIncludeFlags();
includedFlags |= GetNavTerrain(_sourceUnit->GetPositionX(),
_sourceUnit->GetPositionY(),
_sourceUnit->GetPositionZ());
_filter.setIncludeFlags(includedFlags);
}
}
NavTerrain PathGenerator::GetNavTerrain(float x, float y, float z)
{
LiquidData data;
ZLiquidStatus liquidStatus = _sourceUnit->GetBaseMap()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data);
if (liquidStatus == LIQUID_MAP_NO_WATER)
return NAV_GROUND;
switch (data.type_flags)
{
case MAP_LIQUID_TYPE_WATER:
case MAP_LIQUID_TYPE_OCEAN:
return NAV_WATER;
case MAP_LIQUID_TYPE_MAGMA:
return NAV_MAGMA;
case MAP_LIQUID_TYPE_SLIME:
return NAV_SLIME;
default:
return NAV_GROUND;
}
}
bool PathGenerator::HaveTile(const G3D::Vector3& p) const
{
int tx = -1, ty = -1;
float point[VERTEX_SIZE] = {p.y, p.z, p.x};
_navMesh->calcTileLoc(point, &tx, &ty);
/// 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 < 0 || ty < 0)
return false;
return (_navMesh->getTileAt(tx, ty) != NULL);
}
uint32 PathGenerator::FixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath, dtPolyRef const* visited, uint32 nvisited)
{
int32 furthestPath = -1;
int32 furthestVisited = -1;
// Find furthest common polygon.
for (int32 i = npath-1; i >= 0; --i)
{
bool found = false;
for (int32 j = nvisited-1; j >= 0; --j)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
break;
}
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
return npath;
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
uint32 req = nvisited - furthestVisited;
uint32 orig = uint32(furthestPath + 1) < npath ? furthestPath + 1 : npath;
uint32 size = npath > orig ? npath - orig : 0;
if (req + size > maxPath)
size = maxPath-req;
if (size)
memmove(path + req, path + orig, size * sizeof(dtPolyRef));
// Store visited
for (uint32 i = 0; i < req; ++i)
path[i] = visited[(nvisited - 1) - i];
return req+size;
}
bool PathGenerator::GetSteerTarget(float const* startPos, float const* endPos,
float minTargetDist, dtPolyRef const* path, uint32 pathSize,
float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef)
{
// Find steer target.
static const uint32 MAX_STEER_POINTS = 3;
float steerPath[MAX_STEER_POINTS*VERTEX_SIZE];
unsigned char steerPathFlags[MAX_STEER_POINTS];
dtPolyRef steerPathPolys[MAX_STEER_POINTS];
uint32 nsteerPath = 0;
dtStatus dtResult = _navMeshQuery->findStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, (int*)&nsteerPath, MAX_STEER_POINTS);
if (!nsteerPath || DT_SUCCESS != dtResult)
return false;
// Find vertex far enough to steer to.
uint32 ns = 0;
while (ns < nsteerPath)
{
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
!InRangeYZX(&steerPath[ns*VERTEX_SIZE], startPos, minTargetDist, 1000.0f))
break;
ns++;
}
// Failed to find good point to steer to.
if (ns >= nsteerPath)
return false;
dtVcopy(steerPos, &steerPath[ns*VERTEX_SIZE]);
steerPos[1] = startPos[1]; // keep Z value
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
dtStatus PathGenerator::FindSmoothPath(float const* startPos, float const* endPos,
dtPolyRef const* polyPath, uint32 polyPathSize,
float* smoothPath, int* smoothPathSize, uint32 maxSmoothPathSize)
{
*smoothPathSize = 0;
uint32 nsmoothPath = 0;
dtPolyRef polys[MAX_PATH_LENGTH];
memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
uint32 npolys = polyPathSize;
float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
if (DT_SUCCESS != _navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos))
return DT_FAILURE;
if (DT_SUCCESS != _navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos))
return DT_FAILURE;
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys && nsmoothPath < maxSmoothPathSize)
{
// Find location to steer towards.
float steerPos[VERTEX_SIZE];
unsigned char steerPosFlag;
dtPolyRef steerPosRef = INVALID_POLYREF;
if (!GetSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta.
float delta[VERTEX_SIZE];
dtVsub(delta, steerPos, iterPos);
float len = dtSqrt(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;
else
len = SMOOTH_PATH_STEP_SIZE / len;
float moveTgt[VERTEX_SIZE];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[VERTEX_SIZE];
const static uint32 MAX_VISIT_POLY = 16;
dtPolyRef visited[MAX_VISIT_POLY];
uint32 nvisited = 0;
_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
npolys = FixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
result[1] += 0.5f;
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && InRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
break;
}
else if (offMeshConnection && InRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
{
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = INVALID_POLYREF;
dtPolyRef polyRef = polys[0];
uint32 npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (uint32 i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
if (DT_SUCCESS == _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
{
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], startPos);
nsmoothPath++;
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
iterPos[1] += 0.5f;
}
}
// Store results.
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
}
*smoothPathSize = nsmoothPath;
// this is most likely a loop
return nsmoothPath < MAX_POINT_PATH_LENGTH ? DT_SUCCESS : DT_FAILURE;
}
bool PathGenerator::InRangeYZX(const float* v1, const float* v2, float r, float h) const
{
const float dx = v2[0] - v1[0];
const float dy = v2[1] - v1[1]; // elevation
const float dz = v2[2] - v1[2];
return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
}
bool PathGenerator::InRange(G3D::Vector3 const& p1, G3D::Vector3 const& p2, float r, float h) const
{
G3D::Vector3 d = p1 - p2;
return (d.x * d.x + d.y * d.y) < r * r && fabsf(d.z) < h;
}
float PathGenerator::Dist3DSqr(G3D::Vector3 const& p1, G3D::Vector3 const& p2) const
{
return (p1 - p2).squaredLength();
}
Reference in a new issue View git blame Copy permalink