forked from bit/DotRecastNetSim
move utils
This commit is contained in:
ikpil
parent
c333ef7ce6
commit
af6c804330
|
|
@ -0,0 +1,200 @@
|
|||
/*
|
||||
Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
|
||||
recast4j copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org
|
||||
DotRecast Copyright (c) 2023 Choi Ikpil ikpil@naver.com
|
||||
|
||||
This software is provided 'as-is', without any express or implied
|
||||
warranty. In no event will the authors be held liable for any damages
|
||||
arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it
|
||||
freely, subject to the following restrictions:
|
||||
1. The origin of this software must not be misrepresented; you must not
|
||||
claim that you wrote the original software. If you use this software
|
||||
in a product, an acknowledgment in the product documentation would be
|
||||
appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be
|
||||
misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using DotRecast.Core;
|
||||
|
||||
namespace DotRecast.Detour
|
||||
{
|
||||
public static class PathUtils
|
||||
{
|
||||
private const int MAX_STEER_POINTS = 3;
|
||||
|
||||
|
||||
public static SteerTarget getSteerTarget(NavMeshQuery navQuery, Vector3f startPos, Vector3f endPos,
|
||||
float minTargetDist, List<long> path)
|
||||
{
|
||||
// Find steer target.
|
||||
Result<List<StraightPathItem>> result = navQuery.findStraightPath(startPos, endPos, path, MAX_STEER_POINTS, 0);
|
||||
if (result.failed())
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
List<StraightPathItem> straightPath = result.result;
|
||||
float[] steerPoints = new float[straightPath.Count * 3];
|
||||
for (int i = 0; i < straightPath.Count; i++)
|
||||
{
|
||||
steerPoints[i * 3] = straightPath[i].getPos()[0];
|
||||
steerPoints[i * 3 + 1] = straightPath[i].getPos()[1];
|
||||
steerPoints[i * 3 + 2] = straightPath[i].getPos()[2];
|
||||
}
|
||||
|
||||
// Find vertex far enough to steer to.
|
||||
int ns = 0;
|
||||
while (ns < straightPath.Count)
|
||||
{
|
||||
// Stop at Off-Mesh link or when point is further than slop away.
|
||||
if (((straightPath[ns].getFlags() & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
|
||||
|| !inRange(straightPath[ns].getPos(), startPos, minTargetDist, 1000.0f))
|
||||
break;
|
||||
ns++;
|
||||
}
|
||||
|
||||
// Failed to find good point to steer to.
|
||||
if (ns >= straightPath.Count)
|
||||
return null;
|
||||
|
||||
Vector3f steerPos = Vector3f.Of(
|
||||
straightPath[ns].getPos()[0],
|
||||
startPos[1],
|
||||
straightPath[ns].getPos()[2]
|
||||
);
|
||||
int steerPosFlag = straightPath[ns].getFlags();
|
||||
long steerPosRef = straightPath[ns].getRef();
|
||||
|
||||
SteerTarget target = new SteerTarget(steerPos, steerPosFlag, steerPosRef, steerPoints);
|
||||
return target;
|
||||
}
|
||||
|
||||
public static bool inRange(Vector3f v1, Vector3f v2, float r, float h)
|
||||
{
|
||||
float dx = v2[0] - v1[0];
|
||||
float dy = v2[1] - v1[1];
|
||||
float dz = v2[2] - v1[2];
|
||||
return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h;
|
||||
}
|
||||
|
||||
public static List<long> fixupCorridor(List<long> path, List<long> visited)
|
||||
{
|
||||
int furthestPath = -1;
|
||||
int furthestVisited = -1;
|
||||
|
||||
// Find furthest common polygon.
|
||||
for (int i = path.Count - 1; i >= 0; --i)
|
||||
{
|
||||
bool found = false;
|
||||
for (int j = visited.Count - 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 path;
|
||||
|
||||
// Concatenate paths.
|
||||
|
||||
// Adjust beginning of the buffer to include the visited.
|
||||
int req = visited.Count - furthestVisited;
|
||||
int orig = Math.Min(furthestPath + 1, path.Count);
|
||||
int size = Math.Max(0, path.Count - orig);
|
||||
List<long> fixupPath = new List<long>();
|
||||
// Store visited
|
||||
for (int i = 0; i < req; ++i)
|
||||
{
|
||||
fixupPath.Add(visited[(visited.Count - 1) - i]);
|
||||
}
|
||||
|
||||
for (int i = 0; i < size; i++)
|
||||
{
|
||||
fixupPath.Add(path[orig + i]);
|
||||
}
|
||||
|
||||
return fixupPath;
|
||||
}
|
||||
|
||||
// This function checks if the path has a small U-turn, that is,
|
||||
// a polygon further in the path is adjacent to the first polygon
|
||||
// in the path. If that happens, a shortcut is taken.
|
||||
// This can happen if the target (T) location is at tile boundary,
|
||||
// and we're (S) approaching it parallel to the tile edge.
|
||||
// The choice at the vertex can be arbitrary,
|
||||
// +---+---+
|
||||
// |:::|:::|
|
||||
// +-S-+-T-+
|
||||
// |:::| | <-- the step can end up in here, resulting U-turn path.
|
||||
// +---+---+
|
||||
public static List<long> fixupShortcuts(List<long> path, NavMeshQuery navQuery)
|
||||
{
|
||||
if (path.Count < 3)
|
||||
{
|
||||
return path;
|
||||
}
|
||||
|
||||
// Get connected polygons
|
||||
List<long> neis = new List<long>();
|
||||
|
||||
Result<Tuple<MeshTile, Poly>> tileAndPoly = navQuery.getAttachedNavMesh().getTileAndPolyByRef(path[0]);
|
||||
if (tileAndPoly.failed())
|
||||
{
|
||||
return path;
|
||||
}
|
||||
|
||||
MeshTile tile = tileAndPoly.result.Item1;
|
||||
Poly poly = tileAndPoly.result.Item2;
|
||||
|
||||
for (int k = tile.polyLinks[poly.index]; k != NavMesh.DT_NULL_LINK; k = tile.links[k].next)
|
||||
{
|
||||
Link link = tile.links[k];
|
||||
if (link.refs != 0)
|
||||
{
|
||||
neis.Add(link.refs);
|
||||
}
|
||||
}
|
||||
|
||||
// If any of the neighbour polygons is within the next few polygons
|
||||
// in the path, short cut to that polygon directly.
|
||||
int maxLookAhead = 6;
|
||||
int cut = 0;
|
||||
for (int i = Math.Min(maxLookAhead, path.Count) - 1; i > 1 && cut == 0; i--)
|
||||
{
|
||||
for (int j = 0; j < neis.Count; j++)
|
||||
{
|
||||
if (path[i] == neis[j])
|
||||
{
|
||||
cut = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (cut > 1)
|
||||
{
|
||||
List<long> shortcut = new List<long>();
|
||||
shortcut.Add(path[0]);
|
||||
shortcut.AddRange(path.GetRange(cut, path.Count - cut));
|
||||
return shortcut;
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
using DotRecast.Core;
|
||||
|
||||
namespace DotRecast.Detour
|
||||
{
|
||||
public class SteerTarget
|
||||
{
|
||||
public readonly Vector3f steerPos;
|
||||
public readonly int steerPosFlag;
|
||||
public readonly long steerPosRef;
|
||||
public readonly float[] steerPoints;
|
||||
|
||||
public SteerTarget(Vector3f steerPos, int steerPosFlag, long steerPosRef, float[] steerPoints)
|
||||
{
|
||||
this.steerPos = steerPos;
|
||||
this.steerPosFlag = steerPosFlag;
|
||||
this.steerPosRef = steerPosRef;
|
||||
this.steerPoints = steerPoints;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
/*
|
||||
Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
|
||||
recast4j copyright (c) 2021 Piotr Piastucki piotr@jtilia.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied
|
||||
warranty. In no event will the authors be held liable for any damages
|
||||
arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it
|
||||
freely, subject to the following restrictions:
|
||||
1. The origin of this software must not be misrepresented; you must not
|
||||
claim that you wrote the original software. If you use this software
|
||||
in a product, an acknowledgment in the product documentation would be
|
||||
appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be
|
||||
misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
using System;
|
||||
using DotRecast.Core;
|
||||
|
||||
namespace DotRecast.Recast
|
||||
{
|
||||
public static class PolyUtils
|
||||
{
|
||||
public static bool pointInPoly(float[] verts, Vector3f p)
|
||||
{
|
||||
int i, j;
|
||||
bool c = false;
|
||||
for (i = 0, j = verts.Length / 3 - 1; i < verts.Length / 3; j = i++)
|
||||
{
|
||||
Vector3f vi = Vector3f.Of(verts[i * 3], verts[i * 3 + 1], verts[i * 3 + 2]);
|
||||
Vector3f vj = Vector3f.Of(verts[j * 3], verts[j * 3 + 1], verts[j * 3 + 2]);
|
||||
if (((vi[2] > p[2]) != (vj[2] > p[2]))
|
||||
&& (p[0] < (vj[0] - vi[0]) * (p[2] - vi[2]) / (vj[2] - vi[2]) + vi[0]))
|
||||
{
|
||||
c = !c;
|
||||
}
|
||||
}
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
public static int offsetPoly(float[] verts, int nverts, float offset, float[] outVerts, int maxOutVerts)
|
||||
{
|
||||
float MITER_LIMIT = 1.20f;
|
||||
|
||||
int n = 0;
|
||||
|
||||
for (int i = 0; i < nverts; i++)
|
||||
{
|
||||
int a = (i + nverts - 1) % nverts;
|
||||
int b = i;
|
||||
int c = (i + 1) % nverts;
|
||||
int va = a * 3;
|
||||
int vb = b * 3;
|
||||
int vc = c * 3;
|
||||
float dx0 = verts[vb] - verts[va];
|
||||
float dy0 = verts[vb + 2] - verts[va + 2];
|
||||
float d0 = dx0 * dx0 + dy0 * dy0;
|
||||
if (d0 > 1e-6f)
|
||||
{
|
||||
d0 = (float)(1.0f / Math.Sqrt(d0));
|
||||
dx0 *= d0;
|
||||
dy0 *= d0;
|
||||
}
|
||||
|
||||
float dx1 = verts[vc] - verts[vb];
|
||||
float dy1 = verts[vc + 2] - verts[vb + 2];
|
||||
float d1 = dx1 * dx1 + dy1 * dy1;
|
||||
if (d1 > 1e-6f)
|
||||
{
|
||||
d1 = (float)(1.0f / Math.Sqrt(d1));
|
||||
dx1 *= d1;
|
||||
dy1 *= d1;
|
||||
}
|
||||
|
||||
float dlx0 = -dy0;
|
||||
float dly0 = dx0;
|
||||
float dlx1 = -dy1;
|
||||
float dly1 = dx1;
|
||||
float cross = dx1 * dy0 - dx0 * dy1;
|
||||
float dmx = (dlx0 + dlx1) * 0.5f;
|
||||
float dmy = (dly0 + dly1) * 0.5f;
|
||||
float dmr2 = dmx * dmx + dmy * dmy;
|
||||
bool bevel = dmr2 * MITER_LIMIT * MITER_LIMIT < 1.0f;
|
||||
if (dmr2 > 1e-6f)
|
||||
{
|
||||
float scale = 1.0f / dmr2;
|
||||
dmx *= scale;
|
||||
dmy *= scale;
|
||||
}
|
||||
|
||||
if (bevel && cross < 0.0f)
|
||||
{
|
||||
if (n + 2 >= maxOutVerts)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
float d = (1.0f - (dx0 * dx1 + dy0 * dy1)) * 0.5f;
|
||||
outVerts[n * 3 + 0] = verts[vb] + (-dlx0 + dx0 * d) * offset;
|
||||
outVerts[n * 3 + 1] = verts[vb + 1];
|
||||
outVerts[n * 3 + 2] = verts[vb + 2] + (-dly0 + dy0 * d) * offset;
|
||||
n++;
|
||||
outVerts[n * 3 + 0] = verts[vb] + (-dlx1 - dx1 * d) * offset;
|
||||
outVerts[n * 3 + 1] = verts[vb + 1];
|
||||
outVerts[n * 3 + 2] = verts[vb + 2] + (-dly1 - dy1 * d) * offset;
|
||||
n++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (n + 1 >= maxOutVerts)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
outVerts[n * 3 + 0] = verts[vb] - dmx * offset;
|
||||
outVerts[n * 3 + 1] = verts[vb + 1];
|
||||
outVerts[n * 3 + 2] = verts[vb + 2] - dmy * offset;
|
||||
n++;
|
||||
}
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue