/// <summary>
/// Returns the closest point on the navmesh (UNTESTED! EXPERIMENTAL! WILL GO SUPERNOVA ON USE! MAYBE!?)
/// </summary>
public Task <Vector3?> GetClosestPointAsync(Zone zone, Vector3 position, float xRange = 256f, float yRange = 256f, float zRange = 256f)
{
if (!_navmeshPtrs.ContainsKey(zone.ClientID))
{
return(Task.FromResult <Vector3?>(position)); // Assume the point is safe if we don't have a navmesh
}
GSStatistics.Paths.Inc();
return(Task.Factory.StartNew(() =>
{
var ptrs = _navmeshPtrs[zone.ClientID];
var center = (position + Vector3.zAxis * 8).ToRecastFloats();
var outVec = new float[3];
var defaultInclude = (dtPolyFlags.ALL ^ dtPolyFlags.DISABLED);
var defaultExclude = (dtPolyFlags)0;
var filter = new dtPolyFlags[] { defaultInclude, defaultExclude };
var polyPickEx = new Vector3(xRange, yRange, zRange).ToRecastFloats();
var status = FindClosestPoint(ptrs[1], center, polyPickEx, filter, outVec);
if ((status & dtStatus.DT_SUCCESS) == 0)
{
return (Vector3?)null;
}
return new Vector3(outVec[0] * INV_FACTOR, outVec[2] * INV_FACTOR, outVec[1] * INV_FACTOR);
}, TaskCreationOptions.LongRunning));
}
/// <summary>
/// Returns a path that prevents collisions with the navmesh, but floats freely otherwise
/// </summary>
/// <param name="zone"></param>
/// <param name="start">Start in GlobalXYZ</param>
/// <param name="end">End in GlobalXYZ</param>
/// <returns></returns>
public static PathPoint[] GetPathStraight(Zone2 zone, Vector3 start, Vector3 end, dtPolyFlags includeFilter = dtPolyFlags.ALL ^ dtPolyFlags.DISABLED, dtPolyFlags excludeFilter = 0, float polyExtX = 64.0f, float polyExtY = 64.0f, float polyExtZ = 256.0f, dtStraightPathOptions options = dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(null);
}
var ptrs = _loadedZones[zone.ID];
var startFloats = (start + zAxis * 8).ToRecastFloats();
var endFloats = (end + zAxis * 8).ToRecastFloats();
var numNodes = 0;
var buffer = new float[MAX_POLY * 3];
var flags = new dtPolyFlags[MAX_POLY];
var filter = new[] { includeFilter, excludeFilter };
PathStraight(ptrs.queryPtr, startFloats, endFloats, new Vector3(polyExtX, polyExtY, polyExtZ).ToRecastFloats(), filter, options, ref numNodes, buffer, flags);
var points = new PathPoint[numNodes];
var positions = Vector3ArrayFromRecastFloats(buffer, numNodes);
for (var i = 0; i < numNodes; i++)
{
points[i].Position = positions[i];
points[i].Flags = flags[i];
}
return(points);
}
/// <summary>
/// Returns a random point on the navmesh around the given position
/// </summary>
/// <param name="zone">Zone</param>
/// <param name="position">Start in GlobalXYZ</param>
/// <param name="radius">End in GlobalXYZ</param>
/// <returns>null if no point found, Vector3 with point otherwise</returns>
public async Task <Vector3?> GetRandomPointAsync(Zone zone, Vector3 position, float radius)
{
if (!_navmeshPtrs.ContainsKey(zone.ClientID))
{
return(null);
}
GSStatistics.Paths.Inc();
return(await Task.Factory.StartNew(() =>
{
var ptrs = _navmeshPtrs[zone.ClientID];
var center = (position + Vector3.zAxis * 8).ToRecastFloats();
var cradius = (radius *CONVERSION_FACTOR);
var outVec = new float[3];
var defaultInclude = (dtPolyFlags.ALL ^ dtPolyFlags.DISABLED);
var defaultExclude = (dtPolyFlags)0;
var filter = new dtPolyFlags[] { defaultInclude, defaultExclude };
var polyPickEx = new float[3] {
2.0f, 4.0f, 2.0f
};
var status = FindRandomPointAroundCircle(ptrs[1], center, cradius, polyPickEx, filter, outVec);
if ((status & dtStatus.DT_SUCCESS) == 0)
{
return (Vector3?)null;
}
return new Vector3(outVec[0] * INV_FACTOR, outVec[2] * INV_FACTOR, outVec[1] * INV_FACTOR);
}, TaskCreationOptions.LongRunning));
}
public static bool SetPolyFlags(Zone2 zone, uint polyRef, dtPolyFlags flags)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(false);
}
var ptrs = _loadedZones[zone.ID];
var status = SetPolyFlags(ptrs.meshPtr, polyRef, flags);
if ((status & dtStatus.DT_FAILURE) == dtStatus.DT_FAILURE)
{
return(false);
}
return(true);
}
public static bool GetPolyAt(Zone2 zone, Vector3 center, Vector3 extents, dtPolyFlags includeFilter, dtPolyFlags excludeFilter, ref uint polyRef, ref Vector3 point)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(false);
}
float[] outPoint = new float[3];
var ptrs = _loadedZones[zone.ID];
var status = GetPolyAt(ptrs.queryPtr, center.ToRecastFloats(), extents.ToRecastFloats(), new dtPolyFlags[] { includeFilter, excludeFilter }, ref polyRef, outPoint);
point = new Vector3(outPoint[0] * INV_FACTOR, outPoint[2] * INV_FACTOR, outPoint[1] * INV_FACTOR);
if ((status & dtStatus.DT_FAILURE) == dtStatus.DT_FAILURE)
{
return(false);
}
return(true);
}
/// <summary>
/// Returns the closest point on the navmesh (UNTESTED! EXPERIMENTAL! WILL GO SUPERNOVA ON USE! MAYBE!?)
/// </summary>
public static Vector3?GetClosestPoint(Zone2 zone, Vector3 position, float xRange = 256f, float yRange = 256f, float zRange = 256f)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(position);
}
var ptrs = _loadedZones[zone.ID];
var center = (position + zAxis * 8).ToRecastFloats();
var outVec = new float[3];
var defaultInclude = (dtPolyFlags.ALL ^ dtPolyFlags.DISABLED);
var defaultExclude = (dtPolyFlags)0;
var filter = new dtPolyFlags[] { defaultInclude, defaultExclude };
var polyPickEx = new Vector3(xRange, yRange, zRange).ToRecastFloats();
FindClosestPoint(ptrs.queryPtr, center, polyPickEx, filter, outVec);
var result = new Vector3(outVec[0] * INV_FACTOR, outVec[2] * INV_FACTOR, outVec[1] * INV_FACTOR);
return(result == Vector3.Zero ? null : (Vector3?)result);
}
/// <summary>
/// Returns a random point on the navmesh around the given position
/// </summary>
/// <param name="zone">Zone</param>
/// <param name="position">Start in GlobalXYZ</param>
/// <param name="radius">End in GlobalXYZ</param>
/// <returns></returns>
public static Vector3 GetRandomPoint(Zone2 zone, Vector3 position, float radius)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(Vector3.Zero);
}
var ptrs = _loadedZones[zone.ID];
var center = (position + zAxis * 8).ToRecastFloats();
var cradius = (radius * CONVERSION_FACTOR);
var outVec = new float[3];
var defaultInclude = (dtPolyFlags.ALL ^ dtPolyFlags.DISABLED);
var defaultExclude = (dtPolyFlags)0;
var filter = new dtPolyFlags[] { defaultInclude, defaultExclude };
var polyPickEx = new float[3] {
2.0f, 4.0f, 2.0f
};
FindRandomPointAroundCircle(ptrs.queryPtr, center, cradius, polyPickEx, filter, outVec);
return(new Vector3(outVec[0] * INV_FACTOR, outVec[2] * INV_FACTOR, outVec[1] * INV_FACTOR));
}
/// <summary>
/// Returns a path that prevents collisions with the navmesh, but floats freely otherwise
/// </summary>
/// <param name="zone"></param>
/// <param name="start">Start in GlobalXYZ</param>
/// <param name="end">End in GlobalXYZ</param>
/// <returns></returns>
public async Task <WrappedPathingResult> GetPathStraightAsync(Zone zone, Vector3 start, Vector3 end)
{
if (!_navmeshPtrs.ContainsKey(zone.ClientID))
{
return new WrappedPathingResult {
Error = PathingError.NoPathFound,
Points = null,
}
}
;
GSStatistics.Paths.Inc();
return(await Task.Factory.StartNew(() =>
{
var ptrs = _navmeshPtrs[zone.ClientID];
var startFloats = (start + Vector3.zAxis * 8).ToRecastFloats();
var endFloats = (end + Vector3.zAxis * 8).ToRecastFloats();
var numNodes = 0;
var buffer = new float[MAX_POLY * 3];
var flags = new dtPolyFlags[MAX_POLY];
dtPolyFlags includeFilter = dtPolyFlags.ALL ^ dtPolyFlags.DISABLED;
dtPolyFlags excludeFilter = 0;
float polyExtX = 64.0f;
float polyExtY = 64.0f;
float polyExtZ = 256.0f;
dtStraightPathOptions options = dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS;
var filter = new[] { includeFilter, excludeFilter };
var status = PathStraight(ptrs[1], startFloats, endFloats,
new Vector3(polyExtX, polyExtY, polyExtZ).ToRecastFloats(), filter, options, ref numNodes, buffer, flags);
if ((status & dtStatus.DT_SUCCESS) == 0)
{
return new WrappedPathingResult
{
Error = PathingError.NoPathFound,
Points = null,
};
}
var points = new WrappedPathPoint[numNodes];
var positions = Vector3ArrayFromRecastFloats(buffer, numNodes);
for (var i = 0; i < numNodes; i++)
{
points[i].Position = positions[i];
points[i].Flags = flags[i];
}
ImprovePath(zone, points);
if ((status & dtStatus.DT_PARTIAL_RESULT) == 0)
{
return new WrappedPathingResult
{
Error = PathingError.PathFound,
Points = points,
};
}
else
{
return new WrappedPathingResult
{
Error = PathingError.PartialPathFound,
Points = points,
};
}
}, TaskCreationOptions.LongRunning));
}
public override async Task <PathingResponse> GetPathStraight(PathingRequest request, ServerCallContext context)
{
// NOTE: Consider using GetPathStreamed instead
Navmesh mesh;
if (!Navmeshes.TryGetValue(request.Navmesh, out mesh))
{
Metrics.QPSByResult["NavmeshUnavailable"].Increment();
return(new PathingResponse {
ResultCode = PathingResult.NavmeshUnavailable
});
}
var onDequeue = Metrics.Record("Path", request.Navmesh);
return(await Task.Factory.StartNew(() => {
lock (mesh) {
onDequeue();
var startFloats = ToRecastFloats(request.StartingPoint.X, request.StartingPoint.Y, request.StartingPoint.Z + 8);
var endFloats = ToRecastFloats(request.DestinationPoint.X, request.DestinationPoint.Y,
request.DestinationPoint.Z + 8);
var numNodes = 0;
var buffer = new float[MAX_POLY * 3];
var flags = new dtPolyFlags[MAX_POLY];
var includeFilter = dtPolyFlags.ALL ^ dtPolyFlags.DISABLED;
var excludeFilter = (dtPolyFlags)0;
var filter = new[] { includeFilter, excludeFilter };
var polyExt = ToRecastFloats(64f, 64f, 256f);
var options = dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS;
var status = PathStraight(mesh.QueryPtr, startFloats, endFloats, polyExt, filter, options, ref numNodes, buffer, flags);
if ((status & dtStatus.DT_SUCCESS) == 0)
{
Metrics.QPSByResult["NoPathFound"].Increment();
return new PathingResponse {
ResultCode = PathingResult.NoPathFound
};
}
var points = new PathPoint[numNodes];
var positions = Vector3ArrayFromRecastFloats(buffer, numNodes);
for (var i = 0; i < numNodes; i++)
{
points[i] = new PathPoint {
Flags = (uint)flags[i], Position = positions[i]
};
}
if ((status & dtStatus.DT_PARTIAL_RESULT) == 0)
{
Metrics.QPSByResult["PathFound"].Increment();
return new PathingResponse {
ResultCode = PathingResult.PathFound,
Path = { points }
};
}
Metrics.QPSByResult["PartialPathFound"].Increment();
return new PathingResponse {
ResultCode = PathingResult.PartialPathFound,
Path = { points }
};
}
}, TaskCreationOptions.LongRunning));
}
private unsafe PathingResponse GetPathForStreaming(PathingRequest request)
{
Navmesh mesh;
if (!Navmeshes.TryGetValue(request.Navmesh, out mesh))
{
Metrics.QPSByResult["NavmeshUnavailable"].Increment();
return(new PathingResponse()
{
SequenceID = request.SequenceID,
ResultCode = PathingResult.NavmeshUnavailable,
});
}
var onDequeue = Metrics.Record("PathStreamed", request.Navmesh);
lock (mesh)
{
onDequeue();
var startFloats = ToRecastFloats(request.StartingPoint.X, request.StartingPoint.Y, request.StartingPoint.Z + 8);
var endFloats = ToRecastFloats(request.DestinationPoint.X, request.DestinationPoint.Y,
request.DestinationPoint.Z + 8);
var numNodes = 0;
var buffer = new float[MAX_POLY * 3];
var flags = new dtPolyFlags[MAX_POLY];
var includeFilter = dtPolyFlags.ALL ^ dtPolyFlags.DISABLED;
var excludeFilter = (dtPolyFlags)0;
var filter = new[] { includeFilter, excludeFilter };
var polyExt = ToRecastFloats(64f, 64f, 256f);
var options = dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS;
var status = PathStraight(mesh.QueryPtr, startFloats, endFloats, polyExt, filter, options, ref numNodes, buffer, flags);
if ((status & dtStatus.DT_SUCCESS) == 0)
{
Metrics.QPSByResult["NoPathFound"].Increment();
return(new PathingResponse()
{
SequenceID = request.SequenceID,
ResultCode = PathingResult.NoPathFound,
});
}
var code = PathingResult.PathFound;
if ((status & dtStatus.DT_PARTIAL_RESULT) != 0)
{
Metrics.QPSByResult["PartialPathFound"].Increment();
code = PathingResult.PartialPathFound;
}
else
{
Metrics.QPSByResult["PathFound"].Increment();
}
// Create nodes
var positions = Vector3ArrayFromRecastFloats(buffer, numNodes);
var oneStructSize = Marshal.SizeOf <PathPointStruct>();
var oneStruct = Marshal.AllocHGlobal(oneStructSize);
byte[] buf = new byte[oneStructSize * numNodes];
for (int i = 0; i < numNodes; i++)
{
var vec = positions[i];
var pp = new PathPointStruct {
Flags = flags[i],
X = (float)vec.X,
Y = (float)vec.Y,
Z = (float)vec.Z,
};
Marshal.StructureToPtr(pp, oneStruct, false);
Marshal.Copy(oneStruct, buf, i * oneStructSize, oneStructSize);
}
return(new PathingResponse()
{
SequenceID = request.SequenceID,
PathNodes = (uint)numNodes,
ResultCode = code,
SerializedNodes = ByteString.CopyFrom(buf, 0, buf.Length)
});
}
}
private static extern dtStatus SetPolyFlags(IntPtr meshPtr, uint polyRef, dtPolyFlags flags);
public static bool QueryPolygons(Zone2 zone, Vector3 center, Vector3 polyPickExt, dtPolyFlags includeFlags, dtPolyFlags excludeFlags, ref uint[] results, int maxResults = 32)
{
if (!_loadedZones.ContainsKey(zone.ID))
{
return(false);
}
var resultIdBuffer = new uint[maxResults];
var resultCount = 0;
var ptrs = _loadedZones[zone.ID];
var status = QueryPolygons(ptrs.queryPtr, center.ToRecastFloats(), polyPickExt.ToRecastFloats(), new dtPolyFlags[] { includeFlags, excludeFlags }, resultIdBuffer, ref resultCount, maxResults);
if ((status & dtStatus.DT_FAILURE) == dtStatus.DT_FAILURE)
{
return(false);
}
results = new uint[resultCount];
Array.Copy(resultIdBuffer, results, resultCount);
return(true);
}
