private List <NavMeshMovementLine> ReconstructPath(AStarPoint lastPoint, Vector3 end, Vector3 start)
{
reconstructedPath = new List <NavMeshMovementLine>();
reconstructedPath.Add(new NavMeshMovementLine {
point = end
});
reconstructedPath.Add(new NavMeshMovementLine {
point = lastPoint.vert.Position
});
current = lastPoint.parent;
while (current != null)
{
reconstructedPath.Add(new NavMeshMovementLine {
point = current.vert.Position
});
lastPoint = current;
current = lastPoint.parent;
}
reconstructedPath.Add(new NavMeshMovementLine {
point = start
});
reconstructedPath.Reverse();
reconstructedPath = SmoothPath(reconstructedPath);
return(reconstructedPath);
}
private static AStarPoint UpdateAStarPoint(AStarPoint value, float oldG, float distance, Vector3 end, AStarPoint parent = null)
{
float h = (Mathf.Pow((value.vert.Position.x - end.x), 2) + Mathf.Pow((value.vert.Position.y - end.y), 2));
float g = oldG + distance;
value.g = g;
value.h = h;
value.f = h + g;
value.parent = parent;
return(value);
}
public List <NavMeshMovementLine> GetPathFromTo(Vector3 from, Vector3 to)
{
if (actualPoints.Length == 0)
{
return(new List <NavMeshMovementLine>());
}
for (int i = 0; i < actualPoints.Length; i++)
{
actualPoints[i].inClosed = false;
actualPoints[i].f = 0;
actualPoints[i].g = 0;
actualPoints[i].h = 0;
actualPoints[i].parent = null;
}
triStart = navMesh.GetContainingTriangle(from);
if (triStart == null)
{
return(new List <NavMeshMovementLine>());
}
triEnd = navMesh.GetContainingTriangle(to);
if (triEnd == null)
{
return(new List <NavMeshMovementLine>());
}
path.Clear();
// If the points are on the same triangle, send back a straight line between both points
if (triStart.ID == triEnd.ID)
{
path.Add(new NavMeshMovementLine {
point = from
});
path.Add(new NavMeshMovementLine {
point = to
});
return(path);
}
open.Clear();
// Add the starting triangle vertexes to the open list
open.Add(UpdateAStarPoint(GetPointValue(triStart.vertex1.ID), 0, 0, to));
open.Add(UpdateAStarPoint(GetPointValue(triStart.vertex2.ID), 0, 0, to));
open.Add(UpdateAStarPoint(GetPointValue(triStart.vertex3.ID), 0, 0, to));
// The starting point does not need to be kept in the closed list
while (open.Count > 0)
{
open.Sort((a, b) => a.f.CompareTo(b.f));
current = open[0];
if (current.vert.ID == (triEnd.vertex1.ID) ||
current.vert.ID == (triEnd.vertex2.ID) ||
current.vert.ID == (triEnd.vertex3.ID))
{
return(ReconstructPath(current, to, from));
}
open.RemoveAt(0);
current.inClosed = true;
if (open.Count > 4000)
{
break;
}
for (int i = 0; i < current.vert.Count; i++)
{
AStarPoint aStarPointExisting = GetPointValue(current.vert.GetAdjacentVertex(i).ID);
if (aStarPointExisting.inClosed)
{
continue;
}
float cost = current.g + (current.vert.Position - current.vert.GetAdjacentVertex(i).Position).magnitude;
AStarPoint aStarPoint = open.Find((item) => item.vert.Equals(current.vert.GetAdjacentVertex(i)));
if (aStarPoint == null)
{
aStarPoint = UpdateAStarPoint(aStarPointExisting, current.g, (Mathf.Pow((current.vert.Position.x - current.vert.GetAdjacentVertex(i).Position.x), 2) + Mathf.Pow((current.vert.Position.y - current.vert.GetAdjacentVertex(i).Position.y), 2)), to, current);
open.Add(aStarPoint);
}
else
{
if (cost < aStarPoint.g)
{
aStarPoint.g = cost;
aStarPoint.f = aStarPoint.g + aStarPoint.h;
aStarPoint.parent = current;
}
}
}
}
return(path);
}
private static AStarPoint CreateAStarPoint(float oldG, float distance, Vertex current, Vector3 end, AStarPoint parent = null)
{
float h = (Mathf.Pow((current.Position.x - end.x), 2) + Mathf.Pow((current.Position.y - end.y), 2));
float g = oldG + distance;
return(new AStarPoint()
{
vert = current, g = g, h = h, f = h + g, parent = parent
});
}