public static void DrawPolygonGraph(PNavMesh pNavMesh)
{
Color c = Handles.color;
foreach (PNavIsland island in pNavMesh.islands)
{
int index = 0;
foreach (PNavPolygon polygon in island.graph.polygons)
{
Handles.color = Color.green;
Handles.SphereHandleCap(0, (Vector3)polygon.Centroid3D, Quaternion.identity, 0.1f, EventType.Repaint);
Handles.Label((Vector3)polygon.Centroid3D + Vector3.forward * 0.5f, index.ToString());
foreach (PNavEdge edge in polygon.edges)
{
if (edge.hasOther)
{
PNavPolygon self = island.graph.polygons[edge.selfPolygonIndex];
PNavPolygon other = island.graph.polygons[edge.otherPolygonIndex];
Handles.DrawLine((Vector3)self.Centroid3D, (Vector3)other.Centroid3D);
}
}
index++;
}
}
Handles.color = c;
}
public PNavPolygon FindNearestPolygong(Fix64Vec2 point)
{
Fix64 min = Fix64.FromDivision(1000, 1);
PNavPolygon minPolygon = null;
foreach (PNavPolygon polygon in graph.polygons)
{
Fix64 dis = Fix64Vec2.Distance(polygon.centroid, point);
if (dis < min)
{
min = dis;
minPolygon = polygon;
}
}
return(minPolygon);
}
public PNavEdge FindEdge(PNavPolygon currentPolygon, PNavPolygon nextPolygon, Fix64 width)
{
foreach (PNavEdge edge in nextPolygon.edges)
{
if (edge.hasOther && edge.otherPolygonIndex == currentPolygon.index)
{
if (width > edge.width)
{
return(null);
}
else
{
return(edge);
}
}
}
return(null);
}
public static void Process(PNavMesh pNavMesh)
{
using (new SProfiler($"Build polygon graph"))
{
foreach (PNavIsland island in pNavMesh.islands)
{
Dictionary <long, PNavEdge> edgeMap = new Dictionary <long, PNavEdge>();
PNavPolygonGraph graph = new PNavPolygonGraph();
int indiceRead = 0;
for (int p = 0; p < island.polygonCount; p++)
{
int ic = island.indiceCountsOfPolygons[p];
Fix64Vec2[] verts = new Fix64Vec2[ic];
int[] indices = new int[ic];
for (int indiceOfPolygon = 0; indiceOfPolygon < ic; indiceOfPolygon++)
{
int nodeIndex = island.indices[indiceRead];
PNavNode vertNode = island.nodes[nodeIndex];
verts[indiceOfPolygon] = new Fix64Vec2(vertNode.Center.x, vertNode.Center.z);
indices[indiceOfPolygon] = nodeIndex;
indiceRead++;
}
PNavPolygon polygon = new PNavPolygon(verts, ic);
//polygon index should match its index in graph.polygons list
polygon.index = p;
graph.AddPolygon(polygon);
BuildEdges(graph, indices, ic, polygon, edgeMap);
}
island.graph = graph;
}
}
}
public bool PositionOnThePath(Fix64Vec2 position, ref int index)
{
bool positionIsOnThePath = false;
int checkedPolygon = startIndex;
PNavPolygon currentPolygon = null;
while ((MAX_CORNER_COUNT - checkedPolygon) > 0)
{
int polygonIndex = polygonIndexes[checkedPolygon];
PNavPolygon polygon = island.graph.polygons[polygonIndex];
bool insidePolygon = polygon.TestPoint(position);
if (insidePolygon)
{
currentPolygon = polygon;
index = checkedPolygon;
positionIsOnThePath = true;
break;
}
checkedPolygon++;
}
return(positionIsOnThePath);
}
public bool Equals(PNavPolygon other)
{
return(index == other.index);
}
static void BuildEdges(PNavPolygonGraph graph, int[] indices, int count, PNavPolygon polygon, Dictionary <long, PNavEdge> edgeMap)
{
int previous = 0;
Fix64Vec2 previousVector2 = Fix64Vec2.zero;
for (int i = 0; i < count; i++)
{
int current = indices[i];
Fix64Vec2 currentVector2 = polygon.verts[i];
if (i != 0)
{
long key = KeyForIndices(previous, current);
if (edgeMap.ContainsKey(key))
{
PNavEdge edge = edgeMap[key];
edge.otherPolygonIndex = polygon.index;
edge.hasOther = true;
PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);
PNavEdge newEdge = new PNavEdge(edge);
newEdge.SwapDirection();
polygon.edges.Add(newEdge);
}
else
{
PNavEdge edge = new PNavEdge();
edge.selfPolygonIndex = polygon.index;
edge.width = Fix64Vec2.Distance(previousVector2, currentVector2);
edge.pointA = previousVector2;
edge.pointB = currentVector2;
edgeMap[key] = edge;
polygon.edges.Add(edge);
}
}
previous = current;
previousVector2 = currentVector2;
}
int lastCurrent = indices[0];
long lastKey = KeyForIndices(previous, lastCurrent);
if (edgeMap.ContainsKey(lastKey))
{
PNavEdge edge = edgeMap[lastKey];
edge.otherPolygonIndex = polygon.index;
edge.hasOther = true;
PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);
PNavEdge newEdge = new PNavEdge(edge);
newEdge.SwapDirection();
polygon.edges.Add(newEdge);
}
else
{
PNavEdge edge = new PNavEdge();
edge.selfPolygonIndex = polygon.index;
edge.width = Fix64Vec2.Distance(previousVector2, polygon.verts[0]);
edge.pointA = previousVector2;
edge.pointB = polygon.verts[0];
edgeMap[lastKey] = edge;
polygon.edges.Add(edge);
}
}
private void OnDrawGizmos()
{
if (!Application.isPlaying)
{
return;
}
if (_path == null)
{
return;
}
/*
* if (_nextCorner != Fix64Vec3.zero)
* {
* Gizmos.color = Color.red;
* Gizmos.DrawWireSphere((Vector3)_nextCorner, 0.1f);
* }
*/
Gizmos.color = Color.red;
if (_path.Status == ParallelNavMeshPathStatus.Valid)
{
if (_path.startIndex != -1)
{
PNavPolygon previousPolygon = null;
for (int index = _path.startIndex; index < 128; index++)
{
int polygonIndex = _path.polygonIndexes[index];
PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex];
if (previousPolygon != null)
{
Gizmos.DrawLine((Vector3)previousPolygon.Centroid3D, (Vector3)currentPolygon.Centroid3D);
}
previousPolygon = currentPolygon;
}
if (_minLeftVector != Fix64Vec2.zero)
{
Fix64Vec2 pos = _waypoints[_waypoints.Count - 1];
Vector3 pos3D = new Vector3((float)pos.x, 0, (float)pos.y);
Gizmos.color = Color.blue;
Vector3 left3D = new Vector3((float)_minLeft.x, 0, (float)_minLeft.y);
Gizmos.DrawLine(pos3D, left3D);
Gizmos.DrawWireSphere(left3D, 0.1f);
Gizmos.color = Color.magenta;
Vector3 right3D = new Vector3((float)_minRight.x, 0, (float)_minRight.y);
Gizmos.DrawLine(pos3D, right3D);
Gizmos.DrawWireSphere(right3D, 0.1f);
}
}
Gizmos.color = Color.white;
for (int i = 0; i < _waypoints.Count; i++)
{
Fix64Vec2 wp = _waypoints[i];
Vector3 wp3D = new Vector3((float)wp.x, 0, (float)wp.y);
Gizmos.DrawWireSphere(wp3D, 0.1f);
if (i == 0)
{
continue;
}
else
{
Fix64Vec2 prev = _waypoints[i - 1];
Vector3 prevWp3D = new Vector3((float)prev.x, 0, (float)prev.y);
Gizmos.DrawLine(prevWp3D, wp3D);
}
}
}
}
//http://digestingduck.blogspot.com/2010/03/simple-stupid-funnel-algorithm.html
void ProcessNextPolygon()
{
//Debug.Log($"ProcessNextPolygon {_pathPolygonIndex}");
Fix64Vec2 pos = _waypoints[_waypoints.Count - 1];
if (_pathPolygonIndex == 128)
{
//Debug.Log("LAST POLYGON");
if (_minLeftVector == Fix64Vec2.zero)
{
//Debug.Log("GO to destination directly");
_waypoints.Add(_path.Destination2D);
ResetPathValues();
return;
}
Fix64Vec2 destinationVector = _path.Destination2D - pos;
bool destinationLeftOfMinLeft = Fix64Vec2.Cross(_minLeftVector, destinationVector) > Fix64.zero;
bool destinationRightOfMinRight = Fix64Vec2.Cross(_minRightVector, destinationVector) < Fix64.zero;
bool lastRecalculate = false;
bool useLeft = true;
if (destinationLeftOfMinLeft && destinationRightOfMinRight)
{
//when both are too be added
//use the one that is closer to pos
Fix64 leftDis = Fix64Vec2.Distance(_minLeft, pos);
Fix64 rightDis = Fix64Vec2.Distance(_minRight, pos);
if (leftDis > rightDis)
{
useLeft = false;
}
}
if (destinationLeftOfMinLeft && useLeft)
{
_waypoints.Add(_minLeft);
pos = _minLeft;
_pathPolygonIndex = _leftPolygonIndex;
_previousPolygon = _leftPreviousPolygon;
lastRecalculate = true;
}
if (destinationRightOfMinRight && !lastRecalculate)
{
_waypoints.Add(_minRight);
pos = _minRight;
_pathPolygonIndex = _rightPolygonIndex;
_previousPolygon = _rightPreviousPolygon;
lastRecalculate = true;
}
if (!lastRecalculate)
{
//Debug.Log("GO to destination directly");
_waypoints.Add(_path.Destination2D);
ResetPathValues();
}
else
{
_minLeftVector = Fix64Vec2.zero;
}
return;
}
if (_finishedProcessing)
{
return;
}
int polygonIndex = _path.polygonIndexes[_pathPolygonIndex];
PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex];
PNavEdge edge = _path.FindEdge(_previousPolygon, currentPolygon, Fix64.zero);
if (edge == null)
{
Debug.LogError("No Edge found");
return;
}
_previousPolygon = currentPolygon;
_pathPolygonIndex++;
Fix64Vec2 ab = edge.pointB - edge.pointA;
Fix64Vec2 ba = edge.pointA - edge.pointB;
Fix64Vec2 pa = edge.pointA + ab.normalized * error;
Fix64Vec2 pb = edge.pointB + ba.normalized * error;
Fix64Vec2 vA = pa - pos;
Fix64Vec2 vB = pb - pos;
Fix64 c = Fix64Vec2.Cross(vA, vB);
Fix64Vec2 left = pb;
Fix64Vec2 right = pa;
if (c > Fix64.zero)
{
//Debug.Log($"POSITVE: pointA is on the right. pA={pa} pB={pb} pos={pos}");
}
else
{
//Debug.Log($"NEGTIVE: pointA is on the left. pA={pa} pB={pb} pos={pos}");
right = pb;
left = pa;
}
if (_minLeftVector == Fix64Vec2.zero)
{
_minLeft = left;
_minRight = right;
_minLeftVector = left - pos;
_minRightVector = right - pos;
_leftPolygonIndex = _pathPolygonIndex;
_rightPolygonIndex = _pathPolygonIndex;
_leftPreviousPolygon = _previousPolygon;
_rightPreviousPolygon = _previousPolygon;
return;
}
Fix64Vec2 newLeftVector = left - pos;
Fix64Vec2 newRightVector = right - pos;
bool newLeftIsRightToOldRight = Fix64Vec2.Cross(_minRightVector, newLeftVector) < Fix64.zero;
bool newRightIsLeftToOldLeft = Fix64Vec2.Cross(_minLeftVector, newRightVector) > Fix64.zero;
bool recalculateNewVectors = false;
if (newLeftIsRightToOldRight)
{
//move to old right and recalculate
_waypoints.Add(_minRight);
pos = _minRight;
_pathPolygonIndex = _rightPolygonIndex;
_previousPolygon = _rightPreviousPolygon;
recalculateNewVectors = true;
}
if (newRightIsLeftToOldLeft && !recalculateNewVectors)
{
//move to old left and recalculate
_waypoints.Add(_minLeft);
pos = _minLeft;
_pathPolygonIndex = _leftPolygonIndex;
_previousPolygon = _leftPreviousPolygon;
recalculateNewVectors = true;
}
if (recalculateNewVectors)
{
_minLeftVector = Fix64Vec2.zero;
return;
}
else
{
bool newLeftIsOk = Fix64Vec2.Cross(_minLeftVector, newLeftVector) <= Fix64.zero;
bool newRightIsOk = Fix64Vec2.Cross(_minRightVector, newRightVector) >= Fix64.zero;
if (newLeftIsOk)
{
_minLeft = left;
_minLeftVector = newLeftVector;
_leftPolygonIndex = _pathPolygonIndex;
_leftPreviousPolygon = _previousPolygon;
}
if (newRightIsOk)
{
_minRight = right;
_minRightVector = newRightVector;
_rightPolygonIndex = _pathPolygonIndex;
_rightPreviousPolygon = _previousPolygon;
}
}
}
public bool NextCorner1(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index)
{
//if corner not found stay at current position
corner = position;
if (Status != ParallelNavMeshPathStatus.Valid)
{
return(false);
}
if (index < 0)
{
index = startIndex;
}
if (index == MAX_CORNER_COUNT)
{
//reached last polygon, invalidate path
index = -1;
Status = ParallelNavMeshPathStatus.Invalid;
return(false);
}
Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);
int polygonIndex = polygonIndexes[index];
PNavPolygon currentPolygon = island.graph.polygons[polygonIndex];
int nextIndex = index + 1;
if (nextIndex >= MAX_CORNER_COUNT)
{
//reached final polygon
index = MAX_CORNER_COUNT;
corner = Destination;
return(false);
}
int nextPolygonIndex = polygonIndexes[nextIndex];
PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex];
PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width);
if (edge != null)
{
Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, edge.pointA, edge.pointB);
Fix64 distanceToA = Fix64Vec2.Distance(pointOnEdge, edge.pointA);
Fix64 distanceToB = edge.width - distanceToA;
Fix64Vec2 ba = edge.pointB - edge.pointA;
ba = ba.normalized;
Fix64 halfWidth = width / Fix64.two;
if (distanceToA < halfWidth)
{
pointOnEdge = edge.pointA + ba * halfWidth;
}
else if (distanceToB < halfWidth)
{
pointOnEdge = edge.pointB - ba * halfWidth;
}
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
index = nextIndex;
return(true);
}
return(false);
}
public bool NextCorner(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index)
{
//if corner not found stay at current position
corner = position;
if (Status != ParallelNavMeshPathStatus.Valid)
{
return(false);
}
index = BoundaryCheckIndex(index);
if (index == -1)
{
Status = ParallelNavMeshPathStatus.Invalid;
return(false);
}
Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);
Fix64Vec2 pa = Fix64Vec2.zero;
Fix64Vec2 pb = Fix64Vec2.zero;
bool pointABSet = false;
if (polygonIndexes == null)
{
//destination polygon and start polygon is the same
index = MAX_CORNER_COUNT;
}
while (index <= MAX_CORNER_COUNT)
{
if (index == MAX_CORNER_COUNT)
{
//reached final polygon
if (pointABSet)
{
//check if destination is in the span of pa and pb
Fix64Vec2 des = new Fix64Vec2(Destination.x, Destination.z);
Fix64Vec2 vd = des - testPosition;
Fix64Vec2 va = pa - testPosition;
Fix64Vec2 vb = pb - testPosition;
bool desInSpan = Fix64Math.InSpan(vd, va, vb);
if (!desInSpan)
{
Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);
index--;
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
return(true);
}
}
corner = Destination;
break;
}
int nextIndex = index + 1;
int polygonIndex = polygonIndexes[index];
PNavPolygon currentPolygon = island.graph.polygons[polygonIndex];
int nextPolygonIndex = polygonIndexes[nextIndex];
PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex];
PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width);
if (edge != null)
{
if (!pointABSet)
{
pa = edge.pointA;
pb = edge.pointB;
pointABSet = true;
}
else
{
Fix64Vec2 va = pa - testPosition;
Fix64Vec2 vb = pb - testPosition;
Fix64 AXB = Fix64Vec2.Cross(va, vb);
Fix64 BXA = Fix64Vec2.Cross(vb, va);
Fix64Vec2 pa1 = edge.pointA;
Fix64Vec2 pb1 = edge.pointB;
Fix64Vec2 va1 = pa1 - testPosition;
Fix64Vec2 vb1 = pb1 - testPosition;
Fix64 A1XB1 = Fix64Vec2.Cross(va1, vb1);
bool sameDirection = A1XB1 * AXB > Fix64.zero;
bool va1InsideOfSpan = Fix64Math.InSpan(va1, va, vb);
bool vb1InsideOfSpan = Fix64Math.InSpan(vb1, va, vb);
if (va1InsideOfSpan)
{
if (sameDirection)
{
bool valid = IsValidEdgePoint(testPosition, pb, pa1, width);
if (valid)
{
pa = pa1;
}
else
{
va1InsideOfSpan = false;
}
}
else
{
bool valid = IsValidEdgePoint(testPosition, pa, pa1, width);
if (valid)
{
pb = pa1;
}
else
{
va1InsideOfSpan = false;
}
}
}
if (vb1InsideOfSpan)
{
if (sameDirection)
{
bool valid = IsValidEdgePoint(testPosition, pa, pb1, width);
if (valid)
{
pb = pb1;
}
else
{
vb1InsideOfSpan = false;
}
}
else
{
bool valid = IsValidEdgePoint(testPosition, pb, pb1, width);
if (valid)
{
pa = pb1;
}
else
{
vb1InsideOfSpan = false;
}
}
}
if (!va1InsideOfSpan && !vb1InsideOfSpan)
{
Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
index--;
return(true);
}
}
}
else
{
break;
}
index = BoundaryCheckIndex(nextIndex);
}
return(false);
}
public void AddPolygon(PNavPolygon polygon)
{
polygons.Add(polygon);
}
public PNavMeshPath CalculatePath(Fix64Vec3 start, Fix64Vec3 end)
{
Fix64Vec2 startPosition = new Fix64Vec2(start.x, start.z);
Fix64Vec2 endPosition = new Fix64Vec2(end.x, end.z);
PNavPolygon startPolygon = null;
PNavPolygon endPolygon = null;
bool sameIsland = false;
NavMeshAStart astart = null;
PNavMeshPath result = new PNavMeshPath();
result.Destination = end;
result.Destination2D = endPosition;
result.Status = ParallelNavMeshPathStatus.Invalid;
result.navMesh = navMesh;
PNavIsland endIsland = null;
PNavIsland startIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
bool foundStart = false;
bool foundEnd = false;
foreach (PNavPolygon polygon in island.graph.polygons)
{
bool isStart = polygon.TestPoint(startPosition);
bool isEnd = polygon.TestPoint(endPosition);
if (isStart && isEnd)
{
result.Status = ParallelNavMeshPathStatus.Valid;
result.startIndex = -1;
return(result);
}
if (isStart)
{
startPolygon = polygon;
startIsland = island;
foundStart = true;
}
if (isEnd)
{
endPolygon = polygon;
endIsland = island;
foundEnd = true;
}
}
if (foundStart && foundEnd)
{
sameIsland = true;
astart = astartDictionary[island];
result.island = island;
break;
}
if (sameIsland)
{
break;
}
}
if (startPolygon != null && endPolygon == null)
{
//we find the polygon that is closest to the end position in the start position island
sameIsland = true;
astart = astartDictionary[startIsland];
result.island = startIsland;
endPolygon = startIsland.FindNearestPolygong(endPosition);
}
else if (startPolygon == null && endPolygon != null)
{
//we find the polygon that is closest to the start position in the end position island
sameIsland = true;
astart = astartDictionary[endIsland];
result.island = endIsland;
startPolygon = endIsland.FindNearestPolygong(startPosition);
}
else if (startPolygon == null && endPolygon == null)
{
//we find the polygon that is closest to the start position
//we then find the polygon that is closest to the end position in the same island
Fix64 minStart = Fix64.FromDivision(1000, 1);
PNavPolygon minStartPolygon = null;
PNavIsland minStartIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
foreach (PNavPolygon polygon in island.graph.polygons)
{
Fix64 dis = Fix64Vec2.Distance(polygon.centroid, startPosition);
if (dis < minStart)
{
minStart = dis;
minStartPolygon = polygon;
minStartIsland = island;
}
}
}
sameIsland = true;
astart = astartDictionary[minStartIsland];
result.island = minStartIsland;
startPolygon = minStartPolygon;
endPolygon = minStartIsland.FindNearestPolygong(endPosition);
}
if (sameIsland)
{
if (astart != null && startPolygon != null && endPolygon != null)
{
NavMeshAStarNode startNode = astart.FindNode(startPolygon.index);
NavMeshAStarNode endNode = astart.FindNode(endPolygon.index);
NavMeshAStarNode lastNode = null;
using (new SProfiler("Pathfinding"))
{
astart.PrePathFinding();
startNode.startPoint = startPosition;
endNode.isLastNode = true;
endNode.endPoint = endPosition;
lastNode = astart.FindPath(startNode, endNode);
}
result.polygonIndexes = new int[PNavMeshPath.MAX_CORNER_COUNT];
int startIndex = 127;
while (lastNode != null)
{
result.polygonIndexes[startIndex] = lastNode.UserObject.index;
lastNode = (NavMeshAStarNode)lastNode.Parent;
startIndex--;
}
result.startIndex = startIndex + 1;
result.Status = ParallelNavMeshPathStatus.Valid;
}
}
return(result);
}
