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 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); }