public Tuple <bool, Vector3> GetClosestPointOnNavMeshForUnity(Vector3 pos, float distance, Detour.dtQueryFilter filter = null)
{
var startpos = new[] { pos.x, pos.y, pos.z };
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
if (filter == null)
{
filter = new Detour.dtQueryFilter();
}
dtPolyRef startRef = 0;
float[] res = new float[3];
var ret = m_navQuery.findNearestPolyForUnity(startpos, extents, filter, ref startRef, ref res);
if (ret != Detour.DT_SUCCESS)
{
return(new Tuple <bool, Vector3>(false, Vector3.zero));
}
return(new Tuple <bool, Vector3>(true, new Vector3(res[0], res[1], res[2])));
}
public static StraightPath ComputeStraightPath(Detour.dtNavMeshQuery navQuery, float[] startPos, float[] endPos, float distance = 10)
{
//m_ComputedPathType = PathType.Straight;
StraightPath path = new StraightPath();
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endPos, extents, filter, ref endRef, ref endPt);
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path.m_RawPathPolys, ref pathCount, StraightPath.MAX_POLYS);
path.m_RawPathLength = pathCount;
if (pathCount > 0)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float[] epos = new float[3];
Detour.dtVcopy(epos, endPt);
if (path.m_RawPathPolys[pathCount - 1] != endRef)
{
bool posOverPoly = false;
navQuery.closestPointOnPoly(path.m_RawPathPolys[pathCount - 1], endPt, epos, ref posOverPoly);
}
navQuery.findStraightPath(startPt, endPt, path.m_RawPathPolys, pathCount,
path.m_straightPath, path.m_straightPathFlags,
path.m_straightPathPolys, ref path.m_straightPathCount,
StraightPath.MAX_POLYS, path.m_straightPathOptions);
}
return(path);
}
public static uint GetClosestPointOnNavMesh(Detour.dtNavMeshQuery navQuery, float[] pos, ref float[] resPos, float distance = 10)
{
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
dtPolyRef startRef = 0;
resPos = new float[3];
var res = navQuery.findNearestPoly(pos, extents, filter, ref startRef, ref resPos);
return(res);
}
public static TundraVector3 GetClosestPointOnNavMesh(NavMeshQuery navQuery, TundraVector3 pos)
{
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = 10.0f;
}
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
dtPolyRef startRef = 0;
float[] res = new float[3];
navQuery.findNearestPoly(PositionToArray(pos), extents, filter, ref startRef, ref res);
return(ArrayToPosition(res));
}
public static float[] GetClosestPointOnNavMesh(Detour.dtNavMeshQuery navQuery, float[] pos)
{
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = 10.0f;
}
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
dtPolyRef startRef = 0;
float[] res = new float[3];
navQuery.findNearestPoly(pos, extents, filter, ref startRef, ref res);
return(res);
}
public static SmoothPath ComputeSmoothPath(Detour.dtNavMeshQuery navQuery, float[] startWorldPos, float[] endWorldPos, float distance = 10)
{
SmoothPath smoothPath = new SmoothPath();
if (navQuery == null)
{
return(smoothPath);
}
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = distance;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startWorldPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endWorldPos, extents, filter, ref endRef, ref endPt);
const int maxPath = SmoothPath.MAX_POLYS;
dtPolyRef[] path = new dtPolyRef[maxPath];
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path, ref pathCount, maxPath);
smoothPath.m_nsmoothPath = 0;
if (pathCount > 0)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef[] polys = new dtPolyRef[SmoothPath.MAX_POLYS];
for (int i = 0; i < pathCount; ++i)
{
polys[i] = path[i];
}
int npolys = pathCount;
float[] iterPos = new float[3];
float[] targetPos = new float[3];
bool posOverPoly_dummy = false;
navQuery.closestPointOnPoly(startRef, startPt, iterPos, ref posOverPoly_dummy);
navQuery.closestPointOnPoly(polys[npolys - 1], endPt, targetPos, ref posOverPoly_dummy);
const float STEP_SIZE = 0.5f;
const float SLOP = 0.01f;
smoothPath.m_nsmoothPath = 0;
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_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 != 0 && smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
// Find location to steer towards.
float[] steerPos = new float[3];
byte steerPosFlag = 0;
dtPolyRef steerPosRef = 0;
if (!getSteerTarget(navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false;
// Find movement delta.
float[] delta = new float[3]; //, len;
float len = .0f;
Detour.dtVsub(delta, steerPos, iterPos);
len = (float)Mathf.Sqrt(Detour.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 < STEP_SIZE)
{
len = 1;
}
else
{
len = STEP_SIZE / len;
}
float[] moveTgt = new float[3];
Detour.dtVmad(moveTgt, iterPos, delta, len);
// Move
float[] result = new float[3];
dtPolyRef[] visited = new dtPolyRef[16];
int nvisited = 0;
navQuery.moveAlongSurface(polys[0], iterPos, moveTgt, filter,
result, visited, ref nvisited, 16);
npolys = fixupCorridor(polys, npolys, SmoothPath.MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, navQuery);
float h = 0;
dtStatus getHeightStatus = navQuery.getPolyHeight(polys[0], result, ref h);
result[1] = h;
if ((getHeightStatus & Detour.DT_FAILURE) != 0)
{
Debug.LogError("Failed to getPolyHeight " + polys[0] + " pos " + result[0] + " " + result[1] + " " + result[2] + " h " + h);
}
Detour.dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached end of path.
Detour.dtVcopy(iterPos, targetPos);
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached off-mesh connection.
float[] startPos = new float[3]; //, endPos[3];
float[] endPos = new float[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
{
polys[i - npos] = polys[i];
}
npolys -= npos;
// Handle the connection.
dtStatus status = navQuery.getAttachedNavMesh().getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (Detour.dtStatusSucceed(status))
{
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0);
smoothPath.m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if ((smoothPath.m_nsmoothPath & 1) != 0)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0);
smoothPath.m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
Detour.dtVcopy(iterPos, endPos);
float eh = 0.0f;
navQuery.getPolyHeight(polys[0], iterPos, ref eh);
iterPos[1] = eh;
}
}
// Store results.
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
}
}
return(smoothPath);
}
static bool LocCommand(StringArguments args, CommandHandler handler)
{
handler.SendSysMessage("mmap tileloc:");
// grid tile location
Player player = handler.GetPlayer();
int gx = (int)(32 - player.GetPositionX() / MapConst.SizeofGrids);
int gy = (int)(32 - player.GetPositionY() / MapConst.SizeofGrids);
float x, y, z;
player.GetPosition(out x, out y, out z);
handler.SendSysMessage("{0:D4}{1:D2}{2:D2}.mmtile", player.GetMapId(), gy, gx);
handler.SendSysMessage("gridloc [{0}, {1}]", gx, gy);
// calculate navmesh tile location
uint terrainMapId = PhasingHandler.GetTerrainMapId(player.GetPhaseShift(), player.GetMap(), x, y);
Detour.dtNavMesh navmesh = Global.MMapMgr.GetNavMesh(terrainMapId);
Detour.dtNavMeshQuery navmeshquery = Global.MMapMgr.GetNavMeshQuery(terrainMapId, player.GetInstanceId());
if (navmesh == null || navmeshquery == null)
{
handler.SendSysMessage("NavMesh not loaded for current map.");
return(true);
}
float[] min = navmesh.getParams().orig;
float[] location = { y, z, x };
float[] extents = { 3.0f, 5.0f, 3.0f };
int tilex = (int)((y - min[0]) / MapConst.SizeofGrids);
int tiley = (int)((x - min[2]) / MapConst.SizeofGrids);
handler.SendSysMessage("Calc [{0:D2}, {1:D2}]", tilex, tiley);
// navmesh poly . navmesh tile location
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
float[] nothing = new float[3];
ulong polyRef = 0;
if (Detour.dtStatusFailed(navmeshquery.findNearestPoly(location, extents, filter, ref polyRef, ref nothing)))
{
handler.SendSysMessage("Dt [??,??] (invalid poly, probably no tile loaded)");
return(true);
}
if (polyRef == 0)
{
handler.SendSysMessage("Dt [??, ??] (invalid poly, probably no tile loaded)");
}
else
{
Detour.dtMeshTile tile = new Detour.dtMeshTile();
Detour.dtPoly poly = new Detour.dtPoly();
if (Detour.dtStatusSucceed(navmesh.getTileAndPolyByRef(polyRef, ref tile, ref poly)))
{
if (tile != null)
{
handler.SendSysMessage("Dt [{0:D2},{1:D2}]", tile.header.x, tile.header.y);
return(false);
}
}
handler.SendSysMessage("Dt [??,??] (no tile loaded)");
}
return(true);
}
public Queue <Position> FindPathBetween(Position start, Position end, bool useStraightPath = false)
{
var path = new Queue <Position>();
if (dtNavMesh == null)
{
return(path);
}
var startDetourPosition = start.ToDetourPosition();
var endDetourPosition = end.ToDetourPosition();
var queryFilter = new Detour.dtQueryFilter();
var navMeshQuery = new Detour.dtNavMeshQuery();
var status = navMeshQuery.init(dtNavMesh, MAX_PATH);
if (Detour.dtStatusFailed(status))
{
return(path);
}
queryFilter.setIncludeFlags(0xffff);
queryFilter.setExcludeFlags(0x0);
uint startRef = 0;
uint endRef = 0;
float[] startNearest = new float[3];
float[] endNearest = new float[3];
float[] extents = new float[] { 10.0F, 25.0F, 10.0F };
status = navMeshQuery.findNearestPoly(startDetourPosition, extents, queryFilter, ref startRef, ref startNearest);
if (Detour.dtStatusFailed(status))
{
return(path);
}
status = navMeshQuery.findNearestPoly(endDetourPosition, extents, queryFilter, ref endRef, ref endNearest);
if (Detour.dtStatusFailed(status))
{
return(path);
}
if (!dtNavMesh.isValidPolyRef(startRef) || !dtNavMesh.isValidPolyRef(endRef))
{
return(path);
}
uint[] pathPolys = new uint[MAX_PATH];
int pathCount = 0;
float[] straightPath = new float[MAX_PATH * 3];
byte[] straightPathFlags = new byte[MAX_PATH];
uint[] straightPathPolys = new uint[MAX_PATH];
int straightPathCount = 0;
status = navMeshQuery.findPath(
startRef,
endRef,
startNearest,
endNearest,
queryFilter,
pathPolys,
ref pathCount,
MAX_PATH
);
if (Detour.dtStatusFailed(status))
{
path.Enqueue(start);
path.Enqueue(end);
return(path);
}
status = navMeshQuery.findStraightPath(
startNearest,
endNearest,
pathPolys,
pathCount,
straightPath,
straightPathFlags,
straightPathPolys,
ref straightPathCount,
MAX_PATH,
(int)Detour.dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS
);
if (Detour.dtStatusFailed(status))
{
path.Enqueue(start);
path.Enqueue(end);
return(path);
}
if (straightPathCount > 0)
{
if (Detour.dtStatusFailed(status))
{
return(path);
}
for (int i = 3; i < straightPathCount * 3;)
{
float[] pathPos = new float[3];
pathPos[0] = straightPath[i++];
pathPos[1] = straightPath[i++];
pathPos[2] = straightPath[i++];
var position = ToFFXIPosition(pathPos);
path.Enqueue(position);
}
}
else
{
for (int i = 1; i < pathCount; i++)
{
float[] pathPos = new float[3];
bool posOverPoly = false;
if (Detour.dtStatusFailed(navMeshQuery.closestPointOnPoly(pathPolys[i], startDetourPosition, pathPos, ref posOverPoly)))
{
return(path);
}
if (path.Count < 1)
{
if (Detour.dtStatusFailed(navMeshQuery.closestPointOnPolyBoundary(pathPolys[i], startDetourPosition, pathPos)))
{
return(path);
}
}
var position = ToFFXIPosition(pathPos);
path.Enqueue(position);
}
}
if (path.Count < 1)
{
path.Enqueue(end);
}
return(path);
}
public static SmoothPath ComputeSmoothPath(Detour.dtNavMeshQuery navQuery, float[] startWorldPos, float[] endWorldPos)
{
SmoothPath smoothPath = new SmoothPath();
if (navQuery == null){
return smoothPath;
}
float[] extents = new float[3];
for (int i=0;i<3;++i){
extents[i] = 10.0f;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startWorldPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endWorldPos, extents, filter, ref endRef, ref endPt);
const int maxPath = SmoothPath.MAX_POLYS;
dtPolyRef[] path = new dtPolyRef[maxPath];
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path, ref pathCount, maxPath );
smoothPath.m_nsmoothPath = 0;
if (pathCount > 0)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef[] polys = new dtPolyRef[SmoothPath.MAX_POLYS];
for (int i=0;i<pathCount;++i){
polys[i] = path[i];
}
int npolys = pathCount;
float[] iterPos = new float[3];
float[] targetPos = new float[3];
bool posOverPoly_dummy = false;
navQuery.closestPointOnPoly(startRef, startPt, iterPos, ref posOverPoly_dummy);
navQuery.closestPointOnPoly(polys[npolys-1], endPt, targetPos, ref posOverPoly_dummy);
const float STEP_SIZE = 0.5f;
const float SLOP = 0.01f;
smoothPath.m_nsmoothPath = 0;
Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, iterPos, 0);
smoothPath.m_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 != 0 && smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
// Find location to steer towards.
float[] steerPos = new float[3];
byte steerPosFlag = 0;
dtPolyRef steerPosRef = 0;
if (!getSteerTarget(navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, ref steerPosFlag, ref steerPosRef))
break;
bool endOfPath = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false;
// Find movement delta.
float[] delta = new float[3];//, len;
float len = .0f;
Detour.dtVsub(delta, steerPos, iterPos);
len = (float)Mathf.Sqrt(Detour.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 < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float[] moveTgt = new float[3];
Detour.dtVmad(moveTgt, iterPos, delta, len);
// Move
float[] result = new float[3];
dtPolyRef[] visited = new dtPolyRef[16];
int nvisited = 0;
navQuery.moveAlongSurface(polys[0], iterPos, moveTgt, filter,
result, visited, ref nvisited, 16);
npolys = fixupCorridor(polys, npolys, SmoothPath.MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, navQuery);
float h = 0;
dtStatus getHeightStatus = navQuery.getPolyHeight(polys[0], result, ref h);
result[1] = h;
if ((getHeightStatus & Detour.DT_FAILURE) != 0) {
Debug.LogError("Failed to getPolyHeight " + polys[0] + " pos " + result[0] + " " + result[1] + " " + result[2] + " h " + h);
}
Detour.dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached end of path.
Detour.dtVcopy(iterPos, targetPos);
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached off-mesh connection.
float[] startPos = new float[3];//, endPos[3];
float[] endPos = new float[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
dtStatus status = navQuery.getAttachedNavMesh().getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (Detour.dtStatusSucceed(status))
{
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath,smoothPath.m_nsmoothPath*3, startPos, 0);
smoothPath.m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if ((smoothPath.m_nsmoothPath & 1) != 0)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, startPos, 0);
smoothPath.m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
Detour.dtVcopy(iterPos, endPos);
float eh = 0.0f;
navQuery.getPolyHeight(polys[0], iterPos, ref eh);
iterPos[1] = eh;
}
}
// Store results.
if (smoothPath.m_nsmoothPath < SmoothPath.MAX_SMOOTH)
{
Detour.dtVcopy(smoothPath.m_smoothPath, smoothPath.m_nsmoothPath * 3, iterPos, 0);
smoothPath.m_nsmoothPath++;
}
}
}
return smoothPath;
}
public static float[] GetClosestPointOnNavMesh(Detour.dtNavMeshQuery navQuery, float[] pos)
{
float[] extents = new float[3];
for (int i = 0; i < 3; ++i) {
extents[i] = 10.0f;
}
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
dtPolyRef startRef = 0;
float[] res = new float[3];
navQuery.findNearestPoly(pos, extents, filter, ref startRef, ref res);
return res;
}
public static StraightPath ComputeStraightPath(Detour.dtNavMeshQuery navQuery, float[] startPos, float[] endPos)
{
//m_ComputedPathType = PathType.Straight;
StraightPath path = new StraightPath();
float[] extents = new float[3];
for (int i=0;i<3;++i){
extents[i] = 10.0f;
}
dtPolyRef startRef = 0;
dtPolyRef endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly( startPos, extents, filter, ref startRef, ref startPt );
navQuery.findNearestPoly( endPos, extents, filter, ref endRef, ref endPt );
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path.m_RawPathPolys, ref pathCount, StraightPath.MAX_POLYS);
path.m_RawPathLength = pathCount;
if (pathCount > 0)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float[] epos = new float[3];
Detour.dtVcopy(epos, endPt);
if (path.m_RawPathPolys[pathCount - 1] != endRef) {
bool posOverPoly = false;
navQuery.closestPointOnPoly(path.m_RawPathPolys[pathCount - 1], endPt, epos, ref posOverPoly);
}
navQuery.findStraightPath(startPt, endPt, path.m_RawPathPolys, pathCount,
path.m_straightPath, path.m_straightPathFlags,
path.m_straightPathPolys, ref path.m_straightPathCount,
StraightPath.MAX_POLYS, path.m_straightPathOptions);
}
return path;
}
public Queue <Position> FindPathBetween(Position start, Position end, bool useStraightPath = false)
{
var path = new Queue <Position>();
if (dtNavMesh == null)
{
EasyFarm.ViewModels.LogViewModel.Write("FindPathBetween: Unable to path due to lacking navigation mesh for zone " + _zone.ToString());
return(path);
}
var startDetourPosition = start.ToDetourPosition();
var endDetourPosition = end.ToDetourPosition();
var queryFilter = new Detour.dtQueryFilter();
var navMeshQuery = new Detour.dtNavMeshQuery();
var status = navMeshQuery.init(dtNavMesh, 256);
if (Detour.dtStatusFailed(status))
{
return(path);
}
queryFilter.setIncludeFlags(0xffff);
queryFilter.setExcludeFlags(0x0);
uint startRef = 0;
uint endRef = 0;
float[] startNearest = new float[3];
float[] endNearest = new float[3];
float[] extents = new float[] { 10.0F, (float)EasyFarm.UserSettings.Config.Instance.HeightThreshold, 10.0F };
status = navMeshQuery.findNearestPoly(startDetourPosition, extents, queryFilter, ref startRef, ref startNearest);
if (Detour.dtStatusFailed(status))
{
return(path);
}
status = navMeshQuery.findNearestPoly(endDetourPosition, extents, queryFilter, ref endRef, ref endNearest);
if (Detour.dtStatusFailed(status))
{
return(path);
}
if (!dtNavMesh.isValidPolyRef(startRef) || !dtNavMesh.isValidPolyRef(endRef))
{
return(path);
}
uint[] pathPolys = new uint[256];
int pathCount = 0;
status = navMeshQuery.findPath(startRef, endRef, startNearest, endNearest, queryFilter, pathPolys, ref pathCount, 256);
if (Detour.dtStatusFailed(status))
{
return(path);
}
if (path.Count < 1)
{
float[] straightPath = new float[256 * 3];
byte[] straightPathFlags = new byte[256];
uint[] straightPathPolys = new uint[256];
int straightPathCount = 256 * 3;
status = navMeshQuery.findStraightPath(
startNearest,
endNearest,
pathPolys,
pathCount,
straightPath,
straightPathFlags,
straightPathPolys,
ref straightPathCount,
256,
0
);
if (straightPathCount > 1)
{
if (Detour.dtStatusFailed(status))
{
return(path);
}
path.Clear();
// i starts at 3 so the start position is ignored
for (int i = 3; i < straightPathCount * 3;)
{
float[] pathPos = new float[3];
pathPos[0] = straightPath[i++];
pathPos[1] = straightPath[i++];
pathPos[2] = straightPath[i++];
var position = ToFFXIPosition(pathPos);
path.Enqueue(position);
}
}
}
else
{
// i starts at 3 so the start position is ignored
for (int i = 1; i < pathCount; i++)
{
float[] pathPos = new float[3];
bool posOverPoly = false;
if (Detour.dtStatusFailed(navMeshQuery.closestPointOnPoly(pathPolys[i], startDetourPosition, pathPos, ref posOverPoly)))
{
return(path);
}
if (path.Count < 1)
{
if (Detour.dtStatusFailed(navMeshQuery.closestPointOnPolyBoundary(pathPolys[i], startDetourPosition, pathPos)))
{
return(path);
}
}
var position = ToFFXIPosition(pathPos);
path.Enqueue(position);
}
}
return(path);
}
