private IEnumerator WalkToNode(PathNode node)
{
var targetPosition = node.Position.ToVector3();
while (targetPosition != transform.position) {
/* Linearly move player to target cell */
this.transform.position = Vector3.MoveTowards(transform.position, targetPosition, Speed * Time.deltaTime);
var _direction = (targetPosition - transform.position).normalized;
var _lookRotation = Quaternion.LookRotation(_direction);
//rotate us over time according to speed until we are in the required rotation
transform.rotation = Quaternion.Slerp(transform.rotation, _lookRotation, Time.deltaTime * RotationSpeed);
yield return null;
}
}
public IEnumerator FindPath(IntVector3 start, IntVector3 end, Action<bool> callback)
{
currentNode = null;
this.endNode = null;
this.start = start;
this.end = end;
this.openNodes.Clear();
this.closedNodes.Clear();
this.finalNodes.Clear();
/* Create start node and add to open list */
var startNode = new PathNode() {
Position = start,
G = 0,
F = Manhattan(start, end)
};
this.openNodes.Add(startNode);
while (this.openNodes.Count > 0) {
/* Sort open nodes by lowest f(G, H) = G + H */
this.openNodes = this.openNodes.OrderBy( node => node.F ).ToList();
currentNode = this.openNodes.PopAt(0);
this.closedNodes.Add(currentNode);
if (currentNode.Position.Equals(end)) {
this.endNode = currentNode;
break;
}
while (!ContinueSteps && PerformSteps) {
yield return null;
}
ContinueSteps = false;
/* Visit all neighbors of our current node */
foreach(var offset in CellRange.Values2D(false)) {
var absPos = currentNode.Position + offset.ToIntVector3(0);
/* Continue if we already visited that neighbor */
var isClosedNode = this.closedNodes.Find( node => node.Position.Equals(absPos) ) != null;
var isWalkable = CellDefinition.IsWalkable(regionManager.GetCellAt(absPos));
if (isClosedNode || !isWalkable) {
continue;
}
var tentativeG = currentNode.G + Distance(currentNode.Position, absPos);
var openNeighbor = this.openNodes.Find( node => node.Position.Equals(absPos) );
if (openNeighbor == null || tentativeG < openNeighbor.G) {
if (openNeighbor == null) {
openNeighbor = new PathNode();
this.openNodes.Add(openNeighbor);
}
openNeighbor.ParentNode = currentNode;
openNeighbor.Position = absPos;
openNeighbor.G = tentativeG;
openNeighbor.F = openNeighbor.G + Manhattan(absPos, end);
}
}
}
callback(this.endNode != null);
}
private void DrawFinalPathGizmos()
{
var currentNode = this.endNode;
if (this.endNode == null) {
return;
}
Gizmos.color = Color.red;
while (true) {
if (currentNode.ParentNode == null) {
break;
}
Gizmos.DrawLine(currentNode.Position.ToVector3(), currentNode.ParentNode.Position.ToVector3());
currentNode = currentNode.ParentNode;
}
}
private void RemoveNodesInSight(PathNode node)
{
var heightOffset = new Vector3(0, 0.25f, 0);
if (node.ParentNode == null) {
return;
}
if (Physics.Linecast(node.Position + heightOffset, node.ParentNode.Position + heightOffset)) {
RemoveNodesInSight(node.ParentNode);
} else {
var parentIndex = this.finalNodes.IndexOf(node.ParentNode);
if (node.ParentNode == null) {
return;
}
node.ParentNode = node.ParentNode.ParentNode;
this.finalNodes.RemoveAt(parentIndex);
RemoveNodesInSight(node);
}
}
public List<PathNode> ReconstructPath(bool reverse = true)
{
var currentNode = endNode;
if (this.endNode == null) {
Logger.Warn("Cannot reconstruct path, must find it first.");
return null;
}
while (true) {
this.finalNodes.Add(currentNode);
if (currentNode.ParentNode == null) {
break;
}
currentNode = currentNode.ParentNode;
}
if (reverse) {
this.finalNodes.Reverse();
}
return this.finalNodes;
}