public float FindClosestEdge2D(Vector2 pos, out Vector2 hit)
{
hit = new Vector2();
int polyCount = polyNodes.Length;
float min = float.MaxValue;
PolyNode node = null;
float tmpMin;
PolyNode tmpNode;
for (int i = 0; i < polyCount; i++)
{
tmpNode = polyNodes[i];
tmpMin = Vector2.Distance(pos, tmpNode.center);
if (tmpMin < min)
{
min = tmpMin;
node = tmpNode;
}
}
min = float.MaxValue;
int vertCount = node.vertexs.Length;
Vector2 tmpHit;
Vector2 tmpDir;
for (int i = 0; i < vertCount; i++)
{
int j = i + 1;
if (j >= vertCount)
{
j = 0;
}
if (Math.CalculateSegmentIntersect2D(pos, node.center, node.vertexs[i], node.vertexs[j], out tmpHit))
{
tmpMin = Vector2.Distance(node.center, tmpHit);
if (tmpMin <= float.Epsilon)
{
// overlap with center
min = 0f;
break;
}
// move to center slightly
tmpDir = node.center - tmpHit;
tmpDir /= tmpMin;
tmpHit += tmpDir * 0.001f;
if (Math.IsInsideConvexPoly(tmpHit, node.vertexs))
{
tmpMin = Vector2.Distance(pos, tmpHit);
if (tmpMin < min)
{
min = tmpMin;
hit = tmpHit;
}
}
}
}
return(min);
}
public static bool Serialize(NavMeshBuilder builder, out string json)
{
builder.Refresh();
json = "";
List <Vector3> allVerts = new List <Vector3>();
List <int[]> allPolys = new List <int[]>();
foreach (var poly in builder.polys)
{
int[] indexs = new int[poly.vertices.Count];
for (int i = 0; i < indexs.Length; i++)
{
indexs[i] = allVerts.Count + i;
}
allVerts.AddRange(poly.vertices);
allPolys.Add(indexs);
}
List <Vector3> newVerts = new List <Vector3>();
Dictionary <int, int> indexOldToNew = new Dictionary <int, int>();
for (int i = 0; i < allVerts.Count; i++)
{
int existedIndex = -1;
for (int j = 0; j < newVerts.Count; j++)
{
if (Vector3.Distance(allVerts[i], newVerts[j]) < 0.01)
{
existedIndex = j;
break;
}
}
if (existedIndex < 0)
{
indexOldToNew[i] = newVerts.Count;
newVerts.Add(allVerts[i]);
}
else
{
indexOldToNew[i] = existedIndex;
}
}
foreach (var indexs in allPolys)
{
for (int i = 0; i < indexs.Length; i++)
{
indexs[i] = indexOldToNew[indexs[i]];
}
}
int polyCount = allPolys.Count;
PolyNode[] polyNodes = new PolyNode[polyCount];
for (int i = 0; i < polyCount; i++)
{
var indexs = allPolys[i];
PolyNode node = new PolyNode();
node.vertexs = new Vector2[indexs.Length];
for (int j = 0; j < indexs.Length; j++)
{
node.vertexs[j] = new Vector2(newVerts[indexs[j]].x, newVerts[indexs[j]].z);
}
node.center = Math.CalculatePolyCenter2D(node.vertexs);
polyNodes[i] = node;
node.index = i;
// only support convex ploygons
if (!Math.IsConvexPoly(node.vertexs))
{
Debug.LogError("Polygon " + builder.polys[i].name + " is not a convex polygon.", builder.polys[i]);
return(false);
}
// make sure clockwise order
if (!Math.IsClockwise2D(node.vertexs[0], node.vertexs[1], node.vertexs[2]))
{
System.Array.Reverse(node.vertexs);
System.Array.Reverse(indexs);
}
}
// find neighbors
List <PolyNode> neighbors = new List <PolyNode>();
List <int> neighborEdges = new List <int>();
for (int i = 0; i < polyCount; i++)
{
var indexs = allPolys[i];
neighbors.Clear();
neighborEdges.Clear();
for (int j = 0; j < polyCount; j++)
{
if (i == j)
{
continue;
}
var indexs2 = allPolys[j];
int neighborEdge = -1;
for (int m = 0; m < indexs.Length; m++)
{
int e1 = indexs[m];
int e2 = indexs[m + 1 >= indexs.Length ? 0 : m + 1];
for (int n = 0; n < indexs2.Length; n++)
{
int e3 = indexs2[n];
int e4 = indexs2[n + 1 >= indexs2.Length ? 0 : n + 1];
if (e1 == e3 && e2 == e4 || e1 == e4 && e2 == e3)
{
neighborEdge = m;
break;
}
}
if (neighborEdge >= 0)
{
break;
}
}
if (neighborEdge >= 0)
{
neighbors.Add(polyNodes[j]);
neighborEdges.Add(neighborEdge);
}
}
polyNodes[i].neighbors = neighbors.ToArray();
polyNodes[i].neighborEdges = neighborEdges.ToArray();
}
NavMesh navMesh = new NavMesh();
navMesh.polyNodes = polyNodes;
json = NavMesh.Serialize(navMesh);
Debug.Log(json);
return(true);
}
public static void FindPath2D(Vector2 startPos2D, Vector2 endPos2D, PolyNode nodeAStar, List <Vector2> path)
{
path.Clear();
PolyNode origin = nodeAStar;
PolyNode prev = origin;
PolyNode current = prev.parent;
PolyNode next = null;
PolyNode nodeP1 = current;
PolyNode nodeP2 = current;
Vector2 p0 = endPos2D;
Vector2 p1 = Vector2.zero;
Vector2 p2 = Vector2.zero;
Vector2 p3 = Vector2.zero;
Vector2 p4 = Vector2.zero;
int e1, e2, e3, e4;
bool flag1, flag2, flag3, flag4;
bool findCorner = false;
if (current != null)
{
e2 = current.neighborEdges[prev.parentEdge];
e1 = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
p1 = current.vertexs[e1];
p2 = current.vertexs[e2];
while (true)
{
next = current.parent;
if (next == null)
{
flag1 = Math.IsClockwiseMargin2D(p0, p1, startPos2D);
flag2 = Math.IsClockwiseMargin2D(p0, startPos2D, p2);
if (flag1 && flag2)
{
// startPos2D is inside of the ∠p0_p1_p2
break;
}
if (!flag1)
{
// p1 is the next corner
path.Add(p0);
p0 = p1;
origin = nodeP1;
}
else if (!flag2)
{
// p2 is the next corner
path.Add(p0);
p0 = p2;
origin = nodeP2;
}
//reset vector checking from current node
prev = origin;
current = prev.parent;
if (current == null)
{
// origin is the last node
break;
}
nodeP1 = current;
nodeP2 = current;
e2 = current.neighborEdges[prev.parentEdge];
e1 = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
p1 = current.vertexs[e1];
p2 = current.vertexs[e2];
continue;
}
// compare next points in the edge
e4 = next.neighborEdges[current.parentEdge];
e3 = e4 + 1 >= next.vertexs.Length ? 0 : e4 + 1;
p3 = next.vertexs[e3];
p4 = next.vertexs[e4];
flag1 = Math.IsClockwiseMargin2D(p0, p1, p3);
flag2 = Math.IsClockwiseMargin2D(p0, p3, p2);
flag3 = Math.IsClockwiseMargin2D(p0, p1, p4);
flag4 = Math.IsClockwiseMargin2D(p0, p4, p2);
if (flag1 && flag2)
{
// p3 is inside of the ∠p0_p1_p2
p1 = p3;
nodeP1 = next;
}
if (flag3 && flag4)
{
// p4 is inside of the ∠p0_p1_p2
p2 = p4;
nodeP2 = next;
}
findCorner = false;
if (!flag2 && !flag4 || flag1 && !flag2 && flag3)
{
// flag1 && !flag2 && flag3 is the especial case
// p2 is the next corner
path.Add(p0);
p0 = p2;
origin = nodeP2;
findCorner = true;
}
else if (!flag1 && !flag3 || flag1 && flag2 && !flag3)
{
// flag1 && flag2 && !flag3 is the especial case
// p1 is the next corner
path.Add(p0);
p0 = p1;
origin = nodeP1;
findCorner = true;
}
if (findCorner)
{
// reset vector checking from current node
prev = origin;
current = prev.parent;
if (current == null)
{
// origin is the last node
break;
}
nodeP1 = current;
nodeP2 = current;
e2 = current.neighborEdges[prev.parentEdge];
e1 = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
p1 = current.vertexs[e1];
p2 = current.vertexs[e2];
}
else
{
// move to next
prev = current;
current = next;
}
}
}
path.Add(p0);
path.Add(startPos2D);
// reverse because nodeAStar is the end of the node path.
path.Reverse();
}
void OnDrawGizmos()
{
if (navMesh != null)
{
float showHeight = testHeight + offsetHeight;
Gizmos.color = Color.red;
for (int i = 0; i < navMesh.polyNodes.Length; i++)
{
PolyNode node = navMesh.polyNodes[i];
Vector3 size = Vector3.one * 0.1f;
if (testPathNodeEnd != null)
{
PolyNode tmp = testPathNodeEnd;
do
{
if (tmp == node)
{
size.y = 3;
break;
}
tmp = tmp.parent;
}while (tmp != null);
}
Gizmos.DrawCube(new Vector3(node.center.x, showHeight, node.center.y), size);
}
Gizmos.color = Color.green;
float width = testMax.x - testMin.x;
float height = testMax.y - testMin.y;
int row = splitCount;
int column = splitCount;
float deltaColumn = width / column;
float deltaRow = height / row;
Vector2 p = Vector2.zero;
for (int i = 0; i < row; i++)
{
for (int j = 0; j < column; j++)
{
p.x = testMin.x + deltaColumn * j;
p.y = testMin.y + deltaRow * i;
if (navMesh.GetInsidePolyIndex(p) >= 0)
{
Gizmos.DrawRay(new Vector3(p.x, showHeight, p.y), Vector3.up);
}
}
}
if (testPath != null)
{
Gizmos.color = Color.blue;
for (int i = 0; i < testPath.Length; i++)
{
Gizmos.DrawCube(new Vector3(testPath[i].x, showHeight, testPath[i].y), new Vector3(0.2f, 1f, 0.2f));
if (i + 1 < testPath.Length)
{
Gizmos.DrawLine(new Vector3(testPath[i].x, showHeight + 0.1f, testPath[i].y),
new Vector3(testPath[i + 1].x, showHeight + 0.1f, testPath[i + 1].y));
}
}
}
if (testStartClosestDis > 0)
{
Gizmos.color = Color.yellow;
Gizmos.DrawCube(new Vector3(testStartClosest.x, showHeight + 0.1f, testStartClosest.y), new Vector3(0.2f, 10, 0.2f));
}
if (testEndClosestDis > 0)
{
Gizmos.color = Color.magenta;
Gizmos.DrawCube(new Vector3(testEndClosest.x, showHeight + 0.1f, testEndClosest.y), new Vector3(0.2f, 10, 0.2f));
}
}
}
