public AstarNode(Vector3Int pos, AstarNode parent, Vector3Int goal)
{
mPos = pos;
mLevel = parent.GetLevel() + levelWeight;
mPriority = GeneratePriority(goal);
mParent = parent;
}
public AstarNode(Vector3Int pos, int level, int priority, AstarNode parent)
{
mPos = pos;
mLevel = level + levelWeight;
mPriority = priority;
mParent = parent;
}
/// <summary>
/// Sets the target node as the node closest to the user's location selection on the ground mesh.
/// </summary>
/// <returns>The target node.</returns>
/// <param name="target">Target.</param>
/// <param name="t">T.</param>
AstarNode setTargetNode(Vector3 target, AstarNode t)
{
foreach (AstarNode n in roomNodes)
{
if (n.isOccupied)
{
continue;
}
if (t == null)
{
t = n;
t.goal = true;
continue;
}
float newVal = Vector3.Distance(n.getLocation(), target);
float oldVal = Vector3.Distance(t.getLocation(), target);
if (Mathf.Abs(newVal) < Mathf.Abs(oldVal))
{
t.goal = false;
n.goal = true;
t = n;
}
}
return(t);
}
int GetNeighborDis(AstarNode _nodeA, AstarNode _nodeB)
{
float x = Vector2.Distance(_nodeA.NodeGridPos, _nodeB.NodeGridPos);
x *= 10;
return((int)x);
}
public Astar(int x, int y, int tox, int toy, int[,] M, int MX, int MY)//int x,int y,int tox,int toy
{
open = new List <int> ();
close = new List <int> ();
final = new List <AstarNode> ();
finalpath = new List <int[]> ();
ToX = tox;
ToY = toy;
FromX = x;
FromY = y;
AstarNode bg = new AstarNode(x, y);
bg.parentID = -1;
bg.G = 0;
bg.H = (x - tox) * (x - tox) + (y - toy) * (y - toy);
bg.F = bg.G + bg.H;
addToFinal(bg);
addToOpen(CheckInFinal(x, y));
map = M;
MaxX = MX;
MaxY = MY;
MAXLength = 10;
isWalkableFunc = isWalkabeFuncDefault;
//Debug.Log (map[ToX,ToY]);
//Run ();
}
public List <AstarNode> GetNeighbours(AstarNode node)
{
List <AstarNode> neighbours = new List <AstarNode>();
int nx = Mathf.RoundToInt(node.m_gridPos.x);
int ny = Mathf.RoundToInt(node.m_gridPos.y);
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
else
{
int checkX = nx + x;
int checkY = ny + y;
if (checkX >= 0 && checkX < m_sizeX && checkY >= 0 && checkY < m_sizeY)
{
neighbours.Add(m_grid[checkX, checkY]);
}
}
}
}
return(neighbours);
}
void OnDrawGizmos()
{
if (m_debugDraw)
{
Gizmos.DrawWireCube(transform.position, new Vector3(m_size.x, 1, m_size.y));
if (m_grid != null)
{
AstarNode playerNode = GetNodeFromPosition(m_player.transform.position);
if (playerNode == null)
{
print("no player");
}
foreach (AstarNode n in m_grid)
{
Gizmos.color = n.m_walkable ? Color.white : Color.red;
if (m_testPath != null)
{
if (m_testPath.Contains(n))
{
Gizmos.color = Color.blue;
}
}
if (n == playerNode)
{
Gizmos.color = Color.green;
}
Gizmos.DrawWireCube(n.m_position, Vector3.one * (m_nodeRadius * 2 - .1f));
}
}
}
}
private List <Vector2> backpropogate(AstarNode node)
{
List <Vector2> points = new List <Vector2>();
AstarNode prevnode = node;
if (DEBUG)
{
debug_drawv2line(new Vector3(prevnode.x * tilescale, prevnode.y * tilescale), new Vector3(node.from.x * tilescale, node.from.y * tilescale));
}
AstarNode newNode = node.from;
while (newNode.from != null)
{
if (DEBUG)
{
debug_drawv2line(new Vector3(newNode.x * tilescale, newNode.y * tilescale), new Vector3(newNode.from.x * tilescale, newNode.from.y * tilescale));
}
if (newNode.g - newNode.from.g != prevnode.g - newNode.g)
{
// !!CORNER!!!! ! !
points.Add(new Vector3(newNode.x * tilescale, newNode.y * tilescale));
}
prevnode = newNode;
newNode = newNode.from;
}
return(points);
}
private bool BestNodeIsNotFinishNode(AstarNode end)
{
double cost = 99999;
AstarNode tempNode = null;
//AstarNode tempNode2 = null;
for (int i = 0; i < Open.Count; i++)
{
if (Open[i].rank <= cost && !ClosedContainsNode(Open[i]))
{
tempNode = Open[i];
cost = Open[i].rank;
}
}
//tempNode2 = (AstarNode)open2.GetLowestValueNood (open2).abstractObject;
/*
* if (tempNode2.x == end.x && tempNode2.y == end.y)
* {
* return false;
* }
* return true;
*/
///*
if (tempNode.x == end.x && tempNode.y == end.y)
{
return(false);
}
return(true);
//*/
}
private AstarNode GetBestNode()
{
double cost = 99999;
AstarNode tempNode = null;
//AstarNode tempNode2 = null;
for (int i = 0; i < Open.Count; i++)
{
if (Open[i].rank <= cost && !ClosedContainsNode(Open[i]))
{
tempNode = Open[i];
cost = Open[i].rank;
}
}
Open.Remove(tempNode);
//tempNode2 = (AstarNode)open2.GetLowestValueNood (open2).abstractObject;
//open2 = open2.Delete (open2, (int)tempNode2.rank);
//return tempNode2;
//Debug.Log ("Best node returned:" + tempNode.x + ":" + tempNode.y);
return(tempNode);
}
//获取某点的相邻格点
private List <AstarNode> GetNeighborNodes(AstarNode targetNode)
{
List <AstarNode> neighborNodes = new List <AstarNode>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
//TODO 根据移动类型取相邻格点
if (findMapInfo.moveType == MoveType.four && Math.Abs(x) != Math.Abs(y))
{
AstarNode neighborNode = GetNeighborNode(targetNode.coordx + x, targetNode.coordy + y);
if (null != neighborNode && findMapInfo.GetNodeByCoord(neighborNode.coordx, neighborNode.coordy).canWalk)
{
neighborNodes.Add(neighborNode);
}
}
if (findMapInfo.moveType == MoveType.eight)
{
AstarNode neighborNode = GetNeighborNode(targetNode.coordx + x, targetNode.coordy + y);
if (null != neighborNode && findMapInfo.GetNodeByCoord(neighborNode.coordx, neighborNode.coordy).canWalk)
{
neighborNodes.Add(neighborNode);
}
}
}
}
return(neighborNodes);
}
/***************************************************************************/
public List <AstarNode> GetNeighbours(AstarNode node, bool eightConnectivity)
{
List <AstarNode> neighbours = new List <AstarNode>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if ((x == 0 && y == 0))
{
continue;
}
if (!eightConnectivity && (Mathf.Abs(x) + Mathf.Abs(y) > 1))
{
continue;
}
int checkX = node.mGridX + x;
int checkY = node.mGridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbours.Add(grid[checkX, checkY]);
}
}
}
return(neighbours);
}
/***************************************************************************/
float GetDistance(AstarNode nodeA, AstarNode nodeB)
{
// Distance function
//nodeA.mGridX nodeB.mGridX
//nodeA.mGridY nodeB.mGridY
float distance = 0;
switch (heuristic)
{
case Heuristics.Manhattan:
distance = Mathf.Abs(nodeA.mGridX - nodeB.mGridX) + Mathf.Abs(nodeA.mGridY - nodeB.mGridY);
break;
case Heuristics.Euclidean:
distance = Mathf.Sqrt(Mathf.Pow(nodeA.mGridX - nodeB.mGridX, 2) + Mathf.Pow(nodeA.mGridY - nodeB.mGridY, 2));
break;
case Heuristics.Diagonal:
float dx = Mathf.Abs(nodeA.mGridX - nodeB.mGridX);
float dy = Mathf.Abs(nodeA.mGridY - nodeB.mGridY);
float min = Mathf.Min(dx, dy);
float max = Mathf.Max(dx, dy);
float diagonalSteps = min;
float straightSteps = max - min;
distance = Mathf.Sqrt(2) * diagonalSteps + straightSteps;
break;
}
return(distance);
}
int GetManhattenDis(AstarNode _nodeA, AstarNode _nodeB)
{
int x = (int)Mathf.Abs(_nodeA.NodeGridPos.x - _nodeB.NodeGridPos.x);
int y = (int)Mathf.Abs(_nodeA.NodeGridPos.y - _nodeB.NodeGridPos.y);
return(10 * (x + y));
}
List <Vector3> SendPath()
{
List <AstarNode> path = new List <AstarNode>();
List <Vector3> waypoints = new List <Vector3>();
currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Reverse();
Vector2 directionOld = Vector3.zero;
for (int i = 0; i < path.Count; i++)
{
if (i != path.Count - 1)
{
Vector2 directionNew = new Vector2(path[i + 1].xPos - path[i].xPos, path[i + 1].yPos - path[i].yPos);
if (directionOld != directionNew)
{
waypoints.Add(path[i].worldPosition);
directionOld = directionNew;
}
}
else
{
waypoints.Add(path[i].worldPosition);
}
}
return(waypoints);
}
// Update is called once per frame
void Update()
{
AstarNode algotrgtpos = algo();
AstarNode node = algotrgtpos;
List <Vector2> corners = backpropogate(node);
RepelPointsFromObstacles(corners);
//corners.Insert(0, target.gameObject.transform.position);
corners.Insert(0, target.gameObject.transform.position);
//corners.Insert(corners.Count - 1, this.gameObject.transform.position);
corners.Insert(corners.Count, this.gameObject.transform.position);
//Calculate spline points. Only if there are more than 3 points
if (corners.Count > 3)
{
sSpline spline = new sSpline(corners);
Vector2 prevPoint = spline.GetSplinePoint(0f);
Vector2 newPoint;
for (float t = 0; t < (float)spline.points.Count - 3; t += 0.1f)
{
newPoint = spline.GetSplinePoint(t);
Debug.DrawLine(prevPoint, newPoint, Color.yellow);
prevPoint = newPoint;
}
}
open.Clear();
closed.Clear();
}
public AstarNode(AstarNode parent, int x, int y, int gscore, int fscore)
{
Parent = parent;
X = x;
Y = y;
GScore = gscore;
FScore = fscore;
}
public bool compareTo(AstarNode other)
{
if (row == other.getRow() && col == other.getCol())
{
return(true);
}
return(false);
}
public AstarPathFinder(MapGridNode begin, MapGridNode end, MapInfo mapInfo, AstarResult callback)
{
findMapInfo = mapInfo;
targetMapPathFindData = CreatPathFindData(mapInfo);
beginNode = targetMapPathFindData[begin.coordx, begin.coordy];
endNode = targetMapPathFindData[end.coordx, end.coordy];
findResultCallback = callback;
}
/// <summary>
/// Compares the F costs of two AstarNode objects.
/// </summary>
/// <returns>The f.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
int compareF(AstarNode a, AstarNode b)
{
if (a.getF() >= b.getF())
{
return(1);
}
return(-1);
}
void AstarFindPath(Vector3 _startPos, Vector3 _endPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
AstarNode Startnode = grid.NodeFromWorldPosition(_startPos);
AstarNode Endnode = grid.NodeFromWorldPosition(_endPos);
//List<AstarNode> OpenLst = new List<AstarNode>();
Assets.Heap <AstarNode> OpenLst = new Assets.Heap <AstarNode>(grid.MaxSize);
HashSet <AstarNode> ClosedLst = new HashSet <AstarNode>();
OpenLst.add(Startnode);
while (OpenLst.Count > 0)
{
//AstarNode CurNode = OpenLst[0];
//for(int i = 1;i<OpenLst.Count;++i)
//{
// if(OpenLst[i].Fval<=CurNode.Fval&&
// OpenLst[i].Hval<CurNode.Hval)
// {
// CurNode = OpenLst[i];
// }
//}
//OpenLst.Remove(CurNode);
AstarNode CurNode = OpenLst.RemoveFirst();
ClosedLst.Add(CurNode);
if (CurNode == Endnode)
{
sw.Stop();
print("path found in " + sw.ElapsedMilliseconds + "ms");
GetFinalPath(Startnode, Endnode);
return;
}
foreach (AstarNode Neighbor in grid.GetNeighboringNodes(CurNode))
{
if (!Neighbor.wall || ClosedLst.Contains(Neighbor))
{
continue;
}
int movecost = CurNode.Gval + GetNeighborDis(CurNode, Neighbor); /*GetManhattenDis(CurNode, Neighbor)*/;
if (movecost < Neighbor.Gval || !OpenLst.Contains(Neighbor))
{
Neighbor.Gval = movecost;
Neighbor.Hval = GetManhattenDis(Neighbor, Endnode);
Neighbor.parent = CurNode;
if (!OpenLst.Contains(Neighbor))
{
OpenLst.add(Neighbor);
}
else
{
OpenLst.UpdateItem(Neighbor);
}
}
}
}
}
//Manhattan Distance
private float GetDistance(AstarNode firstNode, AstarNode lastNode)
{
//TODO get G or h
int distX = Math.Abs(firstNode.coordx - lastNode.coordx);
int distY = Math.Abs(firstNode.coordy - lastNode.coordy);
//return 14 * distX + 10 * distY;
return(distX + distY);
}
public AstarNode(AstarNode n)
{
pos = new int[2];
pos[0] = n.pos[0]; pos[1] = n.pos[1];
G = n.G;
H = n.H;
F = n.F;
parentID = n.parentID;
}
public List <Vector2Int> FindPath(Vector2Int currentPosition, Vector2Int targetPostion)
{
if (currentPosition == targetPostion)
{
return(new List <Vector2Int>()
{
currentPosition
});
}
Dictionary <Vector2Int, Vector2Int> paths = new Dictionary <Vector2Int, Vector2Int>();
SortedSet <AstarNode> frontier = new SortedSet <AstarNode>(new AstarComparer());
paths.Add(targetPostion, targetPostion);
frontier.Add(new AstarNode(targetPostion, 0, 0));
int iter = 0;
while (frontier.Count > 0)
{
if (iter > 50)
{
break;
}
iter++;
AstarNode current = frontier.Min;
frontier.Remove(current);
if (current.position == currentPosition)
{
break;
}
foreach (Room r in NeighboringRooms(rooms[current.position]))
{
if (paths.ContainsKey(r.coordinates))
{
continue;
}
paths.Add(r.coordinates, current.position);
Vector2Int pred = r.coordinates - currentPosition;
frontier.Add(new AstarNode(r.coordinates, Mathf.Abs(pred.x) + Mathf.Abs(pred.y), current.path + 1));
}
}
if (!paths.ContainsKey(currentPosition))
{
Debug.Log("Pathfinding Error");
return(null);
}
List <Vector2Int> results = new List <Vector2Int>();
Vector2Int p = paths[currentPosition];
while (p != targetPostion)
{
results.Add(p);
p = paths[p];
}
results.Add(p);
return(results);
}
// Private Method
#region Private Method
/// <summary>
/// 길찾기 메소드
/// 1. 시작점과 끝점을 알아낸다
/// </summary>
/// <param name="_startPos">Start Position</param>
/// <param name="_targetPos">End Position</param>
void FindPath(Vector3 _startPos, Vector3 _targetPos)
{
//시작노드
AstarNode StartNode = grid.NodeFromWorldPoint(_startPos);
//목표노드
AstarNode targetNode = grid.NodeFromWorldPoint(_targetPos);
List <AstarNode> openSet = new List <AstarNode>();
HashSet <AstarNode> closedSet = new HashSet <AstarNode>();
openSet.Add(StartNode);
while (openSet.Count > 0)
{
AstarNode currentNode = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
//if (openSet[i].fCost < currentNode.fCost)
//노드 값중 F가 가장 작고 F 가 같다면 H
if (openSet[i].fCost < currentNode.fCost ||
openSet[i].fCost == currentNode.fCost &&
openSet[i].hCost < currentNode.hCost)
{
currentNode = openSet[i];
}
}
// List중에 Open -> Close로 옮기기
openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(StartNode, targetNode);
return;
}
foreach (AstarNode neighbour in grid.GetNeighbors(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.Parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
private double H(AstarNode start, AstarNode end)
{
double D = 1;//, D2 = Mathf.Sqrt (2);
int dx, dy;
dx = Mathf.Abs(start.x - end.x);
dy = Mathf.Abs(start.y - end.y);
//return System.Math.Round((D * (dx + dy) + (D2 - 2 * D) * Mathf.Min(dx, dy)),2);
return(D * (dx + dy));
}
private double MovementCost(AstarNode start, AstarNode end)
{
double D = 1;//, D2 = Mathf.Sqrt (2);
int dx, dy;
dx = Mathf.Abs(start.x - end.x);
dy = Mathf.Abs(start.y - end.y);
//return ((dx > 0) && (dy > 0)) ? System.Math.Round(D2,2) : D;
return(D * (dx + dy));
}
private void Unravel(Result result, AstarNode currentNode)
{
while (currentNode.GetParent() != null)
{
Vector3Int pos = currentNode.GetPos();
result.path.Push(pos);
currentNode = currentNode.GetParent();
}
result.foundPath = true;
}
/// <summary>
/// Creates the path to be followed by recursively passing itself the parent node of the current node.
/// </summary>
/// <param name="n">N.</param>
void createPath(AstarNode n)
{
stackPath.Push(n);
n.inClosedList = false;
n.onPath = true;
if (n.getParent() != null)
{
createPath(n.getParent());
}
}
public void Initialize(AstarNode n)
{
Parent = null;
Neighbors = new List <ScriptableNode>();
Walkable = true;
G = n.G;
H = n.H;
U = n.U;
V = n.V;
Id = n.Id;
}
