public static ActiveNode Create(Short2 node) {
ActiveNode activeNode;
if (poolCount > 0) {
activeNode = pool.Pop();
poolCount -= 1;
}
else {
activeNode = new ActiveNode();
}
activeNode.Node = node;
activeNode.F = 0;
activeNode.G = 0;
activeNode.H = 0;
activeNode.Slot = 0;
activeNode.Parent = null;
return activeNode;
}
public bool HasLineOfSight(Short2 a, Short2 b)
{
var x0 = a.Col;
var y0 = a.Row;
var x1 = b.Col;
var y1 = b.Row;
int dx = Math.Abs(x1 - x0);
int dy = Math.Abs(y1 - y0);
int col = x0;
int row = y0;
int n = 1 + dx + dy;
int x_inc = (x1 > x0) ? 1 : -1;
int y_inc = (y1 > y0) ? 1 : -1;
int error = dx - dy;
dx *= 2;
dy *= 2;
for (; n > 0; --n)
{
if (nodes.IsSet((row * Cols) + col))
{
return(false);
}
if (error > 0)
{
col += x_inc;
error -= dy;
}
else
{
row += y_inc;
error += dx;
}
}
return(true);
}
public static ActiveNode Create(Short2 node)
{
ActiveNode activeNode;
if (poolCount > 0)
{
activeNode = pool.Pop();
poolCount -= 1;
}
else
{
activeNode = new ActiveNode();
}
activeNode.Node = node;
activeNode.F = 0;
activeNode.G = 0;
activeNode.H = 0;
activeNode.Slot = 0;
activeNode.Parent = null;
return(activeNode);
}
public int GetConnections(Short2 node)
{
int r;
int n = 0;
// row above: left, mid and right
r = node.Row - 1;
getConnection(r, node.Col - 1, ref n);
getConnection(r, node.Col, ref n);
getConnection(r, node.Col + 1, ref n);
// same row: left, right
getConnection(node.Row, node.Col - 1, ref n);
getConnection(node.Row, node.Col + 1, ref n);
// row below: left, mid and right
r = node.Row + 1;
getConnection(r, node.Col - 1, ref n);
getConnection(r, node.Col, ref n);
getConnection(r, node.Col + 1, ref n);
return(n);
}
public static int Heuristic(Short2 node, Short2 target)
{
int c_dist = node.Col - target.Col;
int r_dist = node.Row - target.Row;
if (c_dist < 0)
{
c_dist = -c_dist;
}
if (r_dist < 0)
{
r_dist = -r_dist;
}
int h = 0;
if (c_dist > r_dist)
{
h = 14 * r_dist + 10 * (c_dist - r_dist);
}
else
{
h = 14 * c_dist + 10 * (r_dist - c_dist);
}
if (node.Col != target.Col && node.Row != target.Row)
{
h *= 1;
}
else
{
h *= 1;
}
return(h);
}
public bool HasLineOfSight(Short2 a, Short2 b) {
var x0 = a.Col;
var y0 = a.Row;
var x1 = b.Col;
var y1 = b.Row;
int dx = Math.Abs(x1 - x0);
int dy = Math.Abs(y1 - y0);
int col = x0;
int row = y0;
int n = 1 + dx + dy;
int x_inc = (x1 > x0) ? 1 : -1;
int y_inc = (y1 > y0) ? 1 : -1;
int error = dx - dy;
dx *= 2;
dy *= 2;
for (; n > 0; --n) {
if (nodes.IsSet((row * Cols) + col)) {
return false;
}
if (error > 0) {
col += x_inc;
error -= dy;
}
else {
row += y_inc;
error += dx;
}
}
return true;
}
public int GetConnections(Short2 node) {
int r;
int n = 0;
// row above: left, mid and right
r = node.Row - 1;
getConnection(r, node.Col - 1, ref n);
getConnection(r, node.Col, ref n);
getConnection(r, node.Col + 1, ref n);
// same row: left, right
getConnection(node.Row, node.Col - 1, ref n);
getConnection(node.Row, node.Col + 1, ref n);
// row below: left, mid and right
r = node.Row + 1;
getConnection(r, node.Col - 1, ref n);
getConnection(r, node.Col, ref n);
getConnection(r, node.Col + 1, ref n);
return n;
}
public Vector3 ToWorld(Short2 pos) {
return ToWorld(pos.Row, pos.Col);
}
public bool FindPath(Grid grid, Vector3 from, Vector3 to, out Short2[] foundPath) {
return FindPath(grid, grid.ToGrid(from), grid.ToGrid(to), out foundPath);
}
public void Close(Short2 node) {
nodes.Set((node.Row * Cols) + node.Col);
}
public static bool IsDiagonal(Short2 a, Short2 b) {
return a.Row != b.Row && a.Col != b.Col;
}
public static bool IsDiagonal(Short2 a, Short2 b)
{
return(a.Row != b.Row && a.Col != b.Col);
}
public Vector3 ToWorld(Short2 pos)
{
return(ToWorld(pos.Row, pos.Col));
}
public void Open(Short2 node)
{
nodes.Clear((node.Row * Cols) + node.Col);
}
public bool FindPath(Grid grid, Short2 from, Short2 to, out Short2[] foundPath) {
var sw = System.Diagnostics.Stopwatch.StartNew();
foundPath = null;
var found = false;
int totalNodesSearched = 0;
ActiveNode connectionNode = null;
if (grid.IsClosed(to))
return false;
var start = ActiveNode.Create(from);
openList.Push(start);
while (openList.NumberOfItems > 0) {
var current = openList.Pop();
var numConnections = grid.GetConnections(current.Node);
for (var i = 0; i < numConnections; ++i) {
var connection = grid.Connections[i];
if (grid.IsClosed(connection))
continue;
if (closedList.Contains(connection.Packed))
continue;
// Old Node
if (activeNodes.TryGetValue(connection.Packed, out connectionNode)) {
var newG = Grid.Cost(connectionNode, current);
if (newG < connectionNode.G) {
connectionNode.G = newG;
connectionNode.F = connectionNode.H + newG;
connectionNode.Parent = current;
openList.Update(connectionNode);
}
}
// New Node
else {
activeNodes[connection.Packed] = connectionNode = ActiveNode.Create(connection);
connectionNode.H = Grid.Heuristic(connection, to);
connectionNode.G = Grid.Cost(connectionNode, current);
connectionNode.F = connectionNode.H + connectionNode.G;
connectionNode.Parent = current;
openList.Push(connectionNode);
activeList.Add(connectionNode);
++totalNodesSearched;
}
}
if (current.Node.Packed == to.Packed) {
while (current != null) {
path.Add(current.Node);
current = current.Parent;
}
path.Reverse();
foundPath = path.ToArray();
if (grid.AutoSmooth) {
foundPath = grid.Smooth(foundPath);
}
found = true;
break;
}
closedList.Add(current.Node.Packed);
}
for (var i = 0; i < activeList.Count; ++i) {
ActiveNode.Recycle(activeList[i]);
}
sw.Stop();
openList.Clear();
closedList.Clear();
path.Clear();
activeList.Clear();
activeNodes.Clear();
return found;
}
public Short2[] Smooth(Short2[] path) {
if (path != null && path.Length > 2) {
List<Short2> smoothed = new List<Short2>();
Short2 waypoint;
Short2 previous;
waypoint = previous = path[0];
for (var i = 1; i < path.Length; ++i) {
if (!HasLineOfSight(waypoint, path[i])) {
smoothed.Add(waypoint);
waypoint = previous;
}
if (i + 1 == path.Length) {
smoothed.Add(waypoint);
smoothed.Add(path[i]);
}
previous = path[i];
}
return smoothed.ToArray();
}
return path;
}
public static int Heuristic(Short2 node, Short2 target) {
int c_dist = node.Col - target.Col;
int r_dist = node.Row - target.Row;
if (c_dist < 0) {
c_dist = -c_dist;
}
if (r_dist < 0) {
r_dist = -r_dist;
}
int h = 0;
if (c_dist > r_dist) {
h = 14 * r_dist + 10 * (c_dist - r_dist);
}
else {
h = 14 * c_dist + 10 * (r_dist - c_dist);
}
if(node.Col != target.Col && node.Row != target.Row) {
h *= 1;
}
else {
h *= 1;
}
return h;
}
public bool IsClosed(Short2 node)
{
return(nodes.IsSet((node.Row * Cols) + node.Col));
}
public bool IsClosed(Short2 node) {
return nodes.IsSet((node.Row * Cols) + node.Col);
}
public void Close(Short2 node)
{
nodes.Set((node.Row * Cols) + node.Col);
}
public void Open(Short2 node) {
nodes.Clear((node.Row * Cols) + node.Col);
}
public bool FindPath(Grid grid, Short2 from, Short2 to, out Short2[] foundPath)
{
var sw = System.Diagnostics.Stopwatch.StartNew();
foundPath = null;
var found = false;
int totalNodesSearched = 0;
ActiveNode connectionNode = null;
if (grid.IsClosed(to))
{
return(false);
}
var start = ActiveNode.Create(from);
openList.Push(start);
while (openList.NumberOfItems > 0)
{
var current = openList.Pop();
var numConnections = grid.GetConnections(current.Node);
for (var i = 0; i < numConnections; ++i)
{
var connection = grid.Connections[i];
if (grid.IsClosed(connection))
{
continue;
}
if (closedList.Contains(connection.Packed))
{
continue;
}
// Old Node
if (activeNodes.TryGetValue(connection.Packed, out connectionNode))
{
var newG = Grid.Cost(connectionNode, current);
if (newG < connectionNode.G)
{
connectionNode.G = newG;
connectionNode.F = connectionNode.H + newG;
connectionNode.Parent = current;
openList.Update(connectionNode);
}
}
// New Node
else
{
activeNodes[connection.Packed] = connectionNode = ActiveNode.Create(connection);
connectionNode.H = Grid.Heuristic(connection, to);
connectionNode.G = Grid.Cost(connectionNode, current);
connectionNode.F = connectionNode.H + connectionNode.G;
connectionNode.Parent = current;
openList.Push(connectionNode);
activeList.Add(connectionNode);
++totalNodesSearched;
}
}
if (current.Node.Packed == to.Packed)
{
while (current != null)
{
path.Add(current.Node);
current = current.Parent;
}
path.Reverse();
foundPath = path.ToArray();
if (grid.AutoSmooth)
{
foundPath = grid.Smooth(foundPath);
}
found = true;
break;
}
closedList.Add(current.Node.Packed);
}
for (var i = 0; i < activeList.Count; ++i)
{
ActiveNode.Recycle(activeList[i]);
}
sw.Stop();
openList.Clear();
closedList.Clear();
path.Clear();
activeList.Clear();
activeNodes.Clear();
return(found);
}