// this is to compare two points with double precision coordinates
public static bool Identical(Node A, Node B)
{
if (IdenticalCoord(A.x,B.x) && IdenticalCoord(A.y,B.y)) // x and y very close, then point is identical
return true;
else
return false;
}
private int Start(Node startNode)
{
int pathsFound = 0;
Stack<Node> stack = new Stack<Node>();
Stack<Node> pathTraversed = new Stack<Node>();
Node currNode = startNode;
while (currNode != null)
{
if (currNode.IsVisited)
{
currNode = stack.Pop();
continue;
}
pathTraversed.Push(currNode);
stack.Push(currNode);
if (currNode.IsEndNode)
{
if (pathTraversed.Count == Maze.Length - numberOfPits)
{
pathsFound++;
pathTraversed.Pop();
var fs = new StreamWriter("d:\\pf.txt", true);
fs.WriteLine(string.Join("->", pathTraversed.Reverse()));
fs.Close();
currNode = stack.Pop();
currNode.IsVisited = false;
currNode = stack.Pop();
currNode.IsVisited = false;
while (null == (currNode = currNode.getNextBranch()))
{
currNode = stack.Pop();
currNode.IsVisited = false;
}
continue;
}
}
currNode.IsVisited = true;
var nextBranchOfThisNode = currNode = currNode.getNextBranch();
if (nextBranchOfThisNode == null)
{
currNode.IsVisited = false;
currNode = stack.Pop();
}
}
return pathsFound;
}
/// <summary>
/// Creates the final path after it is found
/// </summary>
/// <param name="startTile"></param>
/// <param name="endTile"></param>
public override Vector3[] CreatePath(Node startTile, Node endTile, bool simplified)
{
List<Node> path = new List<Node>();
Node currentTile = endTile;
//Constructs the path by starting at the target position
//and getting the parent of each tile until it gets to the start
while (currentTile != startTile)
{
path.Add(currentTile);
currentTile = currentTile.myParent;
}
path.Add(startTile);
Vector3[] simplifiedPath = Simplify(path, simplified, distort);
Array.Reverse(simplifiedPath);
return simplifiedPath;
}
public NonRecursivePathfinder AddPoints(int[,] matrix)
{
Maze = new Node[matrix.GetLength(0), matrix.GetLength(1)];
for (int i = 0; i < matrix.GetLength(0); i++)
{
for (var j = 0; j < matrix.GetLength(1); j++)
{
Node node = Maze[i, j];
if (node == null)
{
node = new Node(matrix[i, j]);
Maze[i, j] = node;
}
if (!(i - 1 < 0))
Maze[i - 1, j].DownNode = node;
if (!(i + 1 > matrix.GetLength(0) - 1))
{
var downNode = Maze[i + 1, j];
if (downNode == null)
Maze[i + 1, j] = new Node(matrix[i + 1, j]);
Maze[i + 1, j].UpNode = node;
}
if (!(j - 1 < 0))
Maze[i, j - 1].RightNode = node;
if (!(j + 1 > matrix.GetLength(1) - 1))
{
var leftNode = Maze[i, j + 1];
if (leftNode == null)
Maze[i, j + 1] = new Node(matrix[i, j + 1]);
Maze[i, j + 1].LeftNode = node;
}
node.Name = ((char)(65 + i)).ToString() + j.ToString();
if (node.IsPit)
numberOfPits++;
}
}
return this;
}
/// <summary>
/// Gets the adjacent tiles
/// for a given tile
/// </summary>
/// <param name="_t"></param>
/// <returns></returns>
public override List<Node> GetAdjacents(Node _t)
{
List<Node> adjacents = new List<Node>();
for(int x = -1; x <= 1; x++)
{
for(int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0) continue;
int checkX = _t.gridX + x;
int checkY = _t.gridY + y;
if(checkX >= 0 && checkX < sizeX && checkY >= 0 && checkY < sizeY)
{
adjacents.Add(grid[checkX, checkY]);
}
}
}
return adjacents;
}
public Segment(Node node1, Node node2)
{
this.point1 = new Point(node1.x, node1.y);
this.point2 = new Point(node2.x, node2.y);
}
public Segment(Node node1)
{
if (node1.direction == Direction.Horizontal)
{// horizontal node
point1 = new Point(node1.x - 0.5, node1.y);
point2 = new Point(node1.x + 0.5, node1.y);
}
else
{// horizontal node
point1 = new Point(node1.x, node1.y-0.5);
point2 = new Point(node1.x, node1.y+0.5);
}
}
/// <summary> /// Creates the final path after it is found /// </summary> /// <param name="startTile"></param> /// <param name="endTile"></param> public abstract Vector3[] CreatePath(Node startTile, Node endTile, bool simplified);
/// <summary>
/// This function looks through the intersections of a square between weight nodes. It searches clockwise from the angle of entrance from the last segment
/// </summary>
/// <param name="nodes"></param>
/// <param name="startNode"></param>
/// <param name="startAngle"></param>
/// <param name="foundAngle"></param>
/// <returns></returns>
public static Node SearchSegmentEnd(List<Node> nodes, Node startNode, int startAngle, out int foundAngle)
{
// find the end of the segment which starts at "startNode" and had angle coming in "startAngle"
bool found = false;
int foundIndex = -1;
foundAngle = -1;
int i = 0;
// by making this loop run by the number of times there are angles, we permit dead-ends for the segment to go back on itself
for (i = (GetAngleIndex(startAngle) + Angles.Count() - 1) % Angles.Count(); i <= Angles.Count() && found == false; i = (i + Angles.Count() - 1) % Angles.Count())
{
int x = (int)startNode.x;
int ceilX = (int)Math.Ceiling(startNode.x);
int floorX = (int)Math.Floor(startNode.x);
int y = (int)startNode.y;
int ceilY = (int)Math.Ceiling(startNode.y);
int floorY = (int)Math.Floor(startNode.y);
switch (Angles[i])
{
case 0:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x + 1, -1, floorY, ceilY, out foundIndex);
if ( found == true)
{
foundAngle = 0;
}
}
else
{// when vertical we don't have 0 degree search case
found = false;
}
break;
case 45:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x, x + 1, floorY, -1, out foundIndex);
if ( found == true)
{
foundAngle = 45;
}
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, ceilX, -1, y - 1, y, out foundIndex);
if ( found == true)
{
foundAngle = 45;
}
}
break;
case 90:
if (startNode.direction == Direction.Horizontal)
{
found = false;
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, floorX, ceilX, y - 1, -1, out foundIndex);
if ( found == true)
{
foundAngle = 90;
}
}
break;
case 135:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x - 1, x, floorY, -1, out foundIndex);
if ( found == true)
{
foundAngle = 135;
}
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, floorX, -1, y - 1, y, out foundIndex);
if ( found == true)
{
foundAngle = 135;
}
}
break;
case 180:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x - 1, -1, floorY, ceilY, out foundIndex);
if ( found == true)
{
foundAngle = 180;
}
}
else
{// when vertical we don't have 0 degree search case
found = false;
}
break;
case 225:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x - 1, x, ceilY, -1, out foundIndex);
if ( found == true)
{
foundAngle = 225;
}
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, floorX, -1, y, y + 1, out foundIndex);
if ( found == true)
{
foundAngle = 225;
}
}
break;
case 270:
if (startNode.direction == Direction.Horizontal)
{
found = false;
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, floorX, ceilX, y + 1, -1, out foundIndex);
if ( found == true)
{
foundAngle = 270;
}
}
break;
case 315:
if (startNode.direction == Direction.Horizontal)
{
found = SearchNodes(nodes, x, x + 1, ceilY, -1, out foundIndex);
if ( found == true)
{
foundAngle = 315;
}
}
else
{// when vertical we don't have 0 degree search case
found = SearchNodes(nodes, ceilX, -1, y, y + 1, out foundIndex);
if ( found == true)
{
foundAngle = 315;
}
}
break;
}
}
if (found == true)
{
return nodes.ElementAt(foundIndex);
}
else
{
return null;
}
}
public Point[] findPath(Point start, Point end, ref int time)
{
List<Point> p = new List<Point>(); //конечный список
current = new Node(start, 0, 0, 0);
current.H = calcH(start, end);
current.F = current.H;
openList.Add(current);
existsArray[current.P.X, current.P.Y] = 1;
bool ex = false;
bool can = true;
while (!ex)
{
int i = findMinF(openList);
current = openList[i];
openList.RemoveAt(i);
closeList.Add(current);
existsArray[current.P.X, current.P.Y] = 2;
//проверка соседних клеток
Point nb = current.P;
nbPoint(new Point(nb.X + 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y - 1), current, end, x, y);
nbPoint(new Point(nb.X - 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y + 1), current, end, x, y);
if (openList.Count == 0) //открытый списко пустой - путь невозможен
{
ex = true;
can = false;
}
if (existsArray[end.X, end.Y] == 1) //дошли до финала
ex = true;
}
//составление искомого пути
if (can)
{
/*
List<Node> list = new List<Node>();
foreach (Node n in openList)
list.Add(n);
foreach (Node n in closeList)
list.Add(n);
p.Add(end);
Point pp = end;
int i = 0;
while (pp != start)
{
i = inList(pp, list);
if (i < 0)
break;
pp = list[i].Parent;
p.Add(pp);
list.RemoveAt(i);
}*/
p.Add(end);
Point pp = end;
int i = 0;
while (pp != start)
{
if (existsArray[pp.X, pp.Y] == 1)
{
i = inList(pp, openList);
if (i < 0)
break;
if (pp == end)
time = openList[i].G;
pp = openList[i].Parent;
}
else if (existsArray[pp.X, pp.Y] == 2)
{
i = inList(pp, closeList);
if (i < 0)
break;
pp = closeList[i].Parent;
}
p.Add(pp);
}
}
Point[] ret = new Point[p.Count];
for (int i = 0; i < p.Count; i++)
ret[i] = p[p.Count - i - 1];
clear();
return ret;
}
/// <summary>
/// Gets the distance between two tiles
/// Uses Diagonal Distance for calculating
/// the distance
/// </summary>
/// <param name="_currentTile"></param>
/// <param name="_endTile"></param>
/// <returns></returns>
public int GetDistance(Node _currentTile, Node _endTile, DistanceHeuristic _distanceType)
{
int dx = Mathf.Abs(_currentTile.gridX - _endTile.gridX);
int dy = Mathf.Abs(_currentTile.gridY - _endTile.gridY);
///////DEFUALT////////
//Square Grid that allows 8 directions of movement
if (_distanceType == DistanceHeuristic.DIAGONAL)
{
//3/2 are used for calculating the diagonal distance
if (dx > dy)
return 14 * dy + 10 * (dx - dy);
return 14 * dx + 10 * (dy - dx);
}
else
//Square Grid that allows 4 directions of movement
//UP, DOWN, LEFT, RIGHT
if (_distanceType == DistanceHeuristic.MANHATTAN)
{
return 14 * (dx + dy);
}
else
//Square grid that allows any direction of movement
//NOT restricted to a grid, **SLOWER
if (_distanceType == DistanceHeuristic.EUCLIDEAN)
{
return (int)(14 * Mathf.Sqrt(dx * dx + dy * dy));
}
return 0;
}
private int calcG(Node node)
{
int g = current.G;
if (current.P.X == node.P.X && current.P.Y != node.P.Y)
g += 1;
else if (current.P.X != node.P.X && current.P.Y == node.P.Y)
g += 1;
if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.LowDensity)
g += 1;
else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.MediumDensity)
g += 2;
else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.HighDensity)
g += 3;
VG.Map.BloodStream bs = tissue.IsInStream(node.P.X, node.P.Y);
if (bs != null)
{
int dy = node.P.Y - current.P.Y;
int dx = node.P.X - current.P.X;
#region Direction
bool plus = false;
if (bs.Direction == VG.Map.BloodStreamDirection.NorthSouth)
{
if (dy >= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.SouthNorth)
{
if (dy <= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.EstWest)
{
if (dx <= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.WestEst)
{
if (dx >= 0)
plus = false;
else
plus = true;
}
if (plus)
g += 2;
else
g -= 2;
#endregion
}
return g;
}
/// <summary>
/// Traverses a node.
/// </summary>
/// <param name="node"></param>
private void Traverse(Node node)
{
if (node == null)
return;
if (node.IsEntranceNode || node.IsPit || node.IsVisited)
return;
if (node.IsExitNode)
{
pathTraversed.Push(node.Name);
if (pathTraversed.Count == Maze.Length - numberOfPits)
{
var msg = "Path " + pathCount++ + " : " + string.Join("->", pathTraversed.Reverse());
Console.WriteLine(msg);
}
pathTraversed.Pop();
return;
}
pathTraversed.Push(node.Name);
node.IsVisited = true;
Traverse(node.RightNode); //
Traverse(node.DownNode); // Move to Next Node
Traverse(node.LeftNode); //
Traverse(node.UpNode); //
if (node.Name != pathTraversed.Peek())
throw new Exception("Error in Logic.");
node.IsVisited = false;
pathTraversed.Pop();
}
public int CompareTo(Node _t)
{
int value = fScore.CompareTo(_t.fScore);
if (value == 0) value = hScore.CompareTo(_t.hScore);
return -value;
}
public Map()
{
root = new Node();
}
public Map(Node root)
{
this.root = root;
}
protected override void CreateGraph()
{
grid = new Node[sizeX, sizeY];
Vector3 bottomLeftCorner = transform.position - Vector3.right * GraphSize.x / 2 - Vector3.forward * GraphSize.z / 2;
for(int x=0; x < sizeX; x++ )
{
for (int y = 0; y < sizeY; y++)
{
Vector3 worldPoint = bottomLeftCorner + Vector3.right * (x * tileDiameter + tileSizes) + Vector3.forward * (y * tileDiameter + tileSizes);
bool walkable = !(Physics.CheckSphere(worldPoint,tileSizes,myLayer));
grid[x, y] = new Node(walkable, worldPoint, x, y);
//myTiles.Add(grid[x, y]);
}
}
}
private void nbPoint(Point nb, Node current, Point end, int X, int Y)
{
if (nb.X >= X || nb.X < 0)
return;
if (nb.Y >= Y || nb.Y < 0)
return;
// if (ifExistsInList(nb, closeList))
if (existsArray[nb.X, nb.Y] == 2)
return;
if (!tissue.IsInMap(nb.X, nb.Y))
return;
if( tissue[nb.X,nb.Y].AreaType == VG.Map.AreaEnum.Bone )
return;
//if (!ifExistsInList(nb, openList))
if(existsArray[nb.X,nb.Y] !=1 )
{
Node n = new Node(nb, 0, 0, 0);
n.H = calcH(nb, end);
n.G = calcG(n);
n.F = n.G + n.H;
n.Parent = current.P;
openList.Add(n);
existsArray[n.P.X, n.P.Y] = 1;
}
else
{
Node n = openList[inList(nb, openList)];
if (n.G < calcG(n))
{
n.Parent = current.P;
n.G = calcG(n);
n.F = n.G + n.H;
int j = inList(nb, openList);
existsArray[openList[j].P.X, openList[j].P.Y] = 0;
openList.RemoveAt(j);
openList.Add(n);
existsArray[n.P.X, n.P.Y] = 1;
}
}
}
public Point[] findPath(Point start, Point end)
{
List<Point> p = new List<Point>();
current = new Node(start, 0, 0, 0);
current.H = calcH(start, end);
current.F = current.H;
openList.Add(current);
existsArray[current.P.X, current.P.Y] = 1;
bool ex = false;
bool can = true;
while (!ex)
{
int i = findMinF(openList);
current = openList[i];
openList.RemoveAt(i);
closeList.Add(current);
existsArray[current.P.X, current.P.Y] = 2;
Point nb = current.P;
nbPoint(new Point(nb.X + 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y - 1), current, end, x, y);
nbPoint(new Point(nb.X - 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y + 1), current, end, x, y);
if (openList.Count == 0)
{
ex = true;
can = false;
}
//if (ifExistsInList(end, openList))
if (existsArray[end.X, end.Y] == 1)
ex = true;
}
if (can)
{
List<Node> list = new List<Node>();
foreach (Node n in openList)
list.Add(n);
foreach (Node n in closeList)
list.Add(n);
p.Add(end);
Point pp = end;
int i = 0;
while (pp != start)
{
i = inList(pp, list);
if (i < 0)
break;
pp = list[i].Parent;
p.Add(pp);
list.RemoveAt(i);
}
}
Point[] ret = new Point[p.Count];
for (int i = 0; i < p.Count; i++)
ret[i] = p[p.Count - i - 1];
return ret;
}
//функция обрабатывющая "соседнюю" клетку
private void nbPoint(Point nb, Node current, Point end, int X, int Y)
{
//различные проверки клетки - границы массива,закрытый список итд.
if (nb.X >= X || nb.X < 0)
return;
if (nb.Y >= Y || nb.Y < 0)
return;
if (existsArray[nb.X, nb.Y] == 2)
return;
if (!tissue.IsInMap(nb.X, nb.Y))
return;
if( tissue[nb.X,nb.Y].AreaType == VG.Map.AreaEnum.Bone )
return;
//если нет в открытом списке - добавляем
if(existsArray[nb.X,nb.Y] !=1 )
{
Node n = new Node(nb, 0, 0, 0);
n.H = calcH(nb, end);
n.G = calcG(n);
n.F = n.G + n.H;
n.Parent = current.P;
openList.Add(n);
existsArray[n.P.X, n.P.Y] = 1;
}
else //если есть - страдаем некоторой фигней =)
{
Node n = openList[inList(nb, openList)];
if (n.G > calcG(n))
{
n.Parent = current.P;
n.G = calcG(n);
n.F = n.G + n.H;
int j = inList(nb, openList);
existsArray[openList[j].P.X, openList[j].P.Y] = 0;
openList.RemoveAt(j);
openList.Add(n);
existsArray[n.P.X, n.P.Y] = 1;
}
}
}