public void Travel(float amt)
{
while(currNode && currNode.next && amt > 0f) {
float distToNextNode = Vector3.Distance(transform.position, currNode.next.transform.position);
if(amt > distToNextNode) {
amt -= distToNextNode;
if(DestroysChunks && currNode is PathChunkExit) {
currNode.GetComponentInParent<PathChunk>().passed = true;
}
currNode = currNode.next;
transform.position = currNode.transform.position;
transform.eulerAngles = RotateX ? currNode.transform.eulerAngles : new Vector3(0f, currNode.transform.eulerAngles.y, 0f);
} else {
distToNextNode -= amt;
amt = 0f;
float pct = distToNextNode / Vector3.Distance(currNode.transform.position, currNode.next.transform.position);
transform.position = Vector3.Lerp(currNode.next.transform.position, currNode.transform.position, pct);
Vector3 rotEuler = Quaternion.Lerp(currNode.next.transform.rotation, currNode.transform.rotation, pct).eulerAngles;
if(!RotateX) {
rotEuler.x = 0;
}
// Debug.Log(gameObject.name + " rot: " + rotEuler.ToString("2f"));
transform.eulerAngles = rotEuler;
}
}
}
public void fillHFromNodes(PathNode targetNode)
{
foreach (var node in nodes)
{
node.FillH(targetNode);
}
}
public CharacterMoveReport(Character c, Vector3 s, Vector3 d, PathNode eop)
{
character = c;
src = s;
dest = d;
endOfPath = eop;
}
public PathNode()
{
this.prev = null;
this.pos = Vector3.zero;
this.totalDist = 0;
this.distToTarget = float.PositiveInfinity;
}
private bool Search(PathNode currentNode)
{
currentNode.state = NodeState.Closed;
List<PathNode> nextNodes = GetSiblingNodes(currentNode);
// Sort by F-value so that the shortest possible routes are considered first
nextNodes.Sort((node1, node2) => node1.F.CompareTo(node2.F));
foreach (var nextNode in nextNodes)
{
// Check whether the end node has been reached
if (nextNode.location == targetNode.location)
{
return true;
}
else
{
// If not, check the next set of nodes
if (Search(nextNode)) // Note: Recurses back into Search(Node)
return true;
}
}
// The method returns false if this path leads to be a dead end
return false;
}
public List<PathNode> GetPath(PathNode start, PathNode end)
{
WaypointManager wg = GameObject.Find ("WaypointManager").GetComponent<WaypointManager>();
var sources = wg.pathNodes;
if(start == null || end == null)
{
if (sources != null && sources.Count >= 2)
{
start = sources[PathNode.startNode]/*.GetComponent<PathNode>()*/;//.GetInnerObject();
end = sources[PathNode.endNode]/*.GetComponent<PathNode>()*/;//.GetInnerObject();
}
if (start == null || end == null)
{
Debug.LogWarning("Need 'start' and or 'end' defined!");
enabled = false;
return null;
}
}
startIndex = Closest(sources, start.position);
endIndex = Closest(sources, end.position);
return AStarHelper.Calculate(sources[startIndex]/*.GetComponent<PathNode>()*/, sources[endIndex]/*.GetComponent<PathNode>()*/);
}
public PathNode(PathNode newNode)
{
this.prev = newNode.prev;
this.pos = newNode.pos;
this.totalDist = newNode.totalDist;
this.distToTarget = newNode.distToTarget;
}
public static void LoadPathPointDesc (PathNode [] m_NodeList){
TextAsset ta=(TextAsset)Resources.Load ("WayPoint");
string [] sText=ta.text.Split("\n"[0]);
int tLenth=sText.Length;
string sID;
string [] sText2;
int iNeibor=0;
for (int i=0; i<tLenth; i++) {
sID = sText [i];
sID = sID.Trim ();
sText2 = sID.Split (" " [0]);
if (sText2.Length < 1) {
continue;
}
iNeibor = sText2.Length - 1;
sID = sText2 [0];
int iID = int.Parse (sID);
m_NodeList [iID].iNeibors = iNeibor;
m_NodeList [iID].NeiborsNode = new PathNode[iNeibor];
for (int j=0; j<iNeibor; j++) {
sID = sText2 [j + 1];
int iNei = int.Parse (sID);
m_NodeList [i].NeiborsNode [j] = m_NodeList [iNei];
}
}
}
public static PathNode ReversePath(PathNode path)
{
// Get the current first element.
// This will end up being our new last element.
PathNode root = path;
// The last element shouldn't have a backpointer
// so this will start as null.
PathNode next = null;
// While we have elements to reverse...
while (root != null)
{
// Get the next element.
PathNode tmp = root.backPointer;
// Set the current element's backpointer to our previous element.
root.backPointer = next;
// Set the next previous element to the current element.
next = root;
// Set the current element to our new element.
root = tmp;
}
// Return the reversed list.
return next;
}
/// <summary>
/// Finds the shortest path, if any, between 2 nodes given the start node and the target node ID.
/// </summary>
/// <param name="start"></param>
/// <param name="targetid"></param>
/// <returns></returns>
public static Path Find(PathNode start, uint targetid)
{
if (targetid != LastTarget)
foreach (PathNode node in IDNodeMap.Values)
node.Visited = false;
return start.Find(targetid, start);
}
/** Adds a node to the heap */
public void Add(PathNode node) {
if (node == null) throw new System.ArgumentNullException ("Sending null node to BinaryHeap");
if (numberOfItems == binaryHeap.Length) {
int newSize = System.Math.Max(binaryHeap.Length+4,(int)System.Math.Round(binaryHeap.Length*growthFactor));
if (newSize > 1<<18) {
throw new System.Exception ("Binary Heap Size really large (2^18). A heap size this large is probably the cause of pathfinding running in an infinite loop. " +
"\nRemove this check (in BinaryHeap.cs) if you are sure that it is not caused by a bug");
}
PathNode[] tmp = new PathNode[newSize];
for (int i=0;i<binaryHeap.Length;i++) {
tmp[i] = binaryHeap[i];
}
binaryHeap = tmp;
//Debug.Log ("Forced to discard nodes because of binary heap size limit, please consider increasing the size ("+numberOfItems +" "+binaryHeap.Length+")");
//numberOfItems--;
}
PathNode obj = node;
binaryHeap[numberOfItems] = obj;
//node.heapIndex = numberOfItems;//Heap index
int bubbleIndex = numberOfItems;
uint nodeF = node.F;
//Debug.Log ( "Adding node with " + nodeF + " to index " + numberOfItems);
while (bubbleIndex != 0 ) {
int parentIndex = (bubbleIndex-1) / D;
//Debug.Log ("Testing " + nodeF + " < " + binaryHeap[parentIndex].F);
if (nodeF < binaryHeap[parentIndex].F) {
//binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /* \todo Wouldn't it be more efficient with '<' instead of '<=' ? * /
//Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = obj;
//binaryHeap[bubbleIndex].heapIndex = bubbleIndex; //Heap index
//binaryHeap[parentIndex].heapIndex = parentIndex; //Heap index
bubbleIndex = parentIndex;
} else {
break;
}
}
numberOfItems++;
//Validate();
}
private static List<PathNode> GetNeighbours(PathNode pathNode,
Point goal, int[,] field)
{
var result = new List<PathNode>();
// Соседними точками являются соседние по стороне клетки.
Point[] neighbourPoints = new Point[4];
neighbourPoints[0] = new Point(pathNode.Position.x + 1, pathNode.Position.y);
neighbourPoints[1] = new Point(pathNode.Position.x - 1, pathNode.Position.y);
neighbourPoints[2] = new Point(pathNode.Position.x, pathNode.Position.y + 1);
neighbourPoints[3] = new Point(pathNode.Position.x, pathNode.Position.y - 1);
foreach (var point in neighbourPoints)
{
// Проверяем, что не вышли за границы карты.
if (point.x < 0 || point.x >= field.GetLength(0))
continue;
if (point.y < 0 || point.y >= field.GetLength(1))
continue;
// Проверяем, что по клетке можно ходить.
if ((field[point.x, point.y] != 1) && (field[point.x, point.y] != 3))
continue;
// Заполняем данные для точки маршрута.
var neighbourNode = new PathNode()
{
Position = point,
CameFrom = pathNode,
PathLengthFromStart = pathNode.PathLengthFromStart +
GetDistanceBetweenNeighbours(),
HeuristicEstimatePathLength = GetHeuristicPathLength(point, goal)
};
result.Add(neighbourNode);
}
return result;
}
/// <summary>Создаёт новый экземпляр класса <see cref="Movement"/>.</summary>
/// <param name="board">Вся доска.</param>
/// <param name="state">Прямоугольник отрисовки.</param>
/// <param name="current">Текущая ячейка.</param>
/// <param name="speed">Скорость перемещения.</param>
/// <param name="sprite">Анимация объекта.</param>
/// <param name="animation">Параметры анимации объекта.</param>
public Movement(Rectangle state, Cell current, int speed, AnimateSprite sprite, PlayerAnimation animation, bool IsMagic)
{
IsEnd = true;
_CurrentState = state;
_Current = current;
_Speed = speed;
_Tile = sprite;
_Animation = animation;
_Field = Helper.Field;
_Field = new PathNode[Helper.Board.GetLength(0), Helper.Board.GetLength(1)];
bool pass;
for (int i = 0; i < Helper.Board.GetLength(0); i++)
for (int j = 0; j < Helper.Board.GetLength(1); j++)
{
if (IsMagic)
pass = false;
else
pass = (Helper.Board[i, j].Type != CellType.Passable);
_Field[i, j] = new PathNode()
{
IsWall = pass ,
X = Helper.Board[i, j].Rect.X,
Y = Helper.Board[i, j].Rect.Y,
i = i,
j = j,
};
}
}
public void ClearPathfinding()
{
for( int i = 0; i < 4; ++i )
{
PathNodes[i] = new PathNode();
}
}
private void ResetGame() {
DestroyAllNodes ();
pathNodeBoundsRect = new Rect ();
targetPathLength = 0;
isGameOver = false;
timeUntilGameEnds = 45f;
scoreText.enabled = false;
gameOverText.enabled = false;
// Make the first TWO nodes.
sourceNode = Instantiate (pathNodePrefab).GetComponent<PathNode> ();
sourceNode.Initialize (this, Vector2.zero, null, 10, 0, 0, true, true);
PathNode secondNode = Instantiate (pathNodePrefab).GetComponent<PathNode> ();
secondNode.Initialize (this, new Vector2(0,-0.5f), null, 4, 50, 0, true, true);
sourceNode.nextNodes.Add (secondNode);
secondNode.previousNode = sourceNode;
// Reset travelers!
DestroyAllTravelers ();
// travelers = new List<Traveler> ();
// int NUM_TRAVELERS = 8;
// for (int i=0; i<NUM_TRAVELERS; i++) {
// AddTraveler();
// Traveler newTraveler = Instantiate (travelerPrefab).GetComponent<Traveler>();
// float charge = i<NUM_TRAVELERS*0.5f ? -1 : 1;
// newTraveler.Initialize (this, worldGenerator.sourceNode, charge);
// travelers.Add (newTraveler);
// }
}
public void SetVals(PathNode prev, Vector3 pos, float dist, float dist2)
{
this.prev = prev;
this.pos = pos;
this.totalDist = dist;
this.distToTarget = dist2;
}
public virtual BinaryHeapNode Insert(PathNode data)
{
Nodes.Add(new BinaryHeapNode(data));
FilterUp(Count - 1);
return null;
}
//安裝Tag是WP的物件
void SetupWPNode(){
GameObject [] point = GameObject.FindGameObjectsWithTag ("WP");
int pLenth = point.Length;
m_NodeList = new PathNode[pLenth];
string tNodeName = "";
string [] s;
int iWP;
for (int i = 0; i < pLenth; i++) {
PathNode pNode = new PathNode ();
pNode.iNeibors = 0;
pNode.NeiborsNode = null;
pNode.fH = 0.0f;
pNode.fG = 0.0f;
pNode.fF = 0.0f;
pNode.tParent = null;
pNode.tPoint = point [i].transform.position;
tNodeName = point [i].name;
s = tNodeName.Split ('_');
iWP = int.Parse (s [1]);
pNode.iID = iWP;
m_NodeList [iWP] = pNode;
}
}
static void Main(string[] args)
{
string str = FileReader.ReadFile(FILENAME);
string[] strArray = str.Replace("\n", "|").Split('|');
int matrixWidth = strArray.Length;
PathNode[][] matrix = new PathNode[matrixWidth][];
for (int i = 0; i < strArray.Length; i++)
{
matrix[i] = new PathNode[matrixWidth];
string line = strArray[i];
string[] numStrs = line.Split(',');
for (int j = 0; j < numStrs.Length; j++)
{
string numStr = numStrs[j];
matrix[i][j] = new PathNode() { NodeValue = int.Parse(numStr), NodePathSum = 0 };
}
}
matrix[matrixWidth - 1][matrixWidth - 1].NodePathSum = matrix[matrixWidth - 1][matrixWidth - 1].NodeValue;
int currSub = 1;
while (currSub < matrixWidth)
{
// 从下向上计算左侧
int colIndex = matrixWidth - currSub - 1;
int tempRowIndex = 0;
for (int i = 0; i < currSub; i++)
{
tempRowIndex = matrixWidth - i - 1;
// 下方
long tempDownSum = long.MaxValue;
if (i > 0)
{
tempDownSum = matrix[tempRowIndex + 1][colIndex].NodePathSum;
}
// 右侧
long tempRightSum = matrix[tempRowIndex][colIndex + 1].NodePathSum;
matrix[tempRowIndex][colIndex].NodePathSum = matrix[tempRowIndex][colIndex].NodeValue + Math.Min(tempDownSum, tempRightSum);
}
// 从右向左计算顶部
int rowIndex = matrixWidth - currSub - 1;
int tempColumnIndex = 0;
for (int i = 0; i < currSub; i++)
{
tempColumnIndex = matrixWidth - i - 1;
// 下方
long tempDownSum = matrix[rowIndex + 1][tempColumnIndex].NodePathSum;
// 右侧
long tempRightSum = long.MaxValue;
if (i > 0)
{
tempRightSum = matrix[rowIndex][tempColumnIndex + 1].NodePathSum;
}
matrix[rowIndex][tempColumnIndex].NodePathSum = matrix[rowIndex][tempColumnIndex].NodeValue + Math.Min(tempDownSum, tempRightSum);
}
// 计算左上角
matrix[rowIndex][colIndex].NodePathSum = matrix[rowIndex][colIndex].NodeValue + Math.Min(matrix[rowIndex + 1][colIndex].NodePathSum, matrix[rowIndex][colIndex + 1].NodePathSum);
currSub++;
}
Console.WriteLine("Result is {0}", matrix[0][0].NodePathSum);
}
public static bool IsClearPath(PathNode node1, PathNode node2)
{
float dist = Vector3.Distance(node1.transform.position, node2.transform.position);
Vector3 direction = node2.transform.position - node1.transform.position;
direction.Normalize();
return !Physics.Raycast(node1.transform.position, direction, dist, int.MaxValue);
}
public void SetNext(PathNode node)
{
if(m_next!=null){
m_next.m_parent=null;
}
m_next=node;
node.m_parent=this;
}
public void createPathNode(GameObject square, GameObject goalSquare)
{
thisSquare = square;
this.goalSquare = goalSquare;
neighbors = findNeighbors();
parent = null;
// square.GetComponent<Renderer>().material.color = Color.red;
}
public PathNode ToPathNode()
{
PathNode node;
node = new PathNode(mapXPosition, mapYPosition,rect.Width, walkable);
return node;
}
public virtual void CharacterMovedIncremental(Character c, Vector3 src, Vector3 dest, PathNode endOfPath)
{
map.BroadcastMessage(
"MovedCharacterIncremental",
new CharacterMoveReport(c, src, dest, endOfPath),
SendMessageOptions.DontRequireReceiver
);
}
public Edge GetEdge(PathNode a, PathNode b)
{
foreach (Edge e in edges)
{
if ((e.anchorA == a && e.anchorB == b) || (e.anchorA == b && e.anchorB == a))
return e;
}
return null;
}
public void UpdateDestinations(PathNode[] dests)
{
FindMap();
destinations = dests;
positions = map.CoalesceTiles(destinations);
overlayTex = BoundsTextureFor(overlayTex, positions);
shadeMaterial.SetTexture("_Boxes", overlayTex);
//SetSelectedPoints(selectedPoints);
}
/// <summary>
/// 设置节点
/// </summary>
/// <param name="node">下一个节点</param>
public void SetNode(PathNode node)
{
if (nextNode != null)
{
nextNode.currentNode = null;
nextNode = node;
currentNode = this;
}
}
public void tryAlternative(PathNode alt)
{
float altG = alt.g + 1;
if(altG < g){
parent = alt;
g = altG;
f = g + h;
}
}
public PathNode(int ID, PathNode parentNode, uint navCellIndex, float cost, float total)
{
m_ID = ID;
m_parentNode = parentNode;
m_navCellIndex = navCellIndex;
m_cost = cost;
m_total = total;
m_setMembership = eSetMembership.open_set;
}
// класс для вычисления маршрута.
public static List<Point> FindPath(AreaType[,] field, Point start, Point goal)
{
// Шаг 1.
var closedSet = new Collection<PathNode>();
var openSet = new Collection<PathNode>();
// Шаг 2.
PathNode startNode = new PathNode()
{
Position = start,
CameFrom = null,
PathLengthFromStart = 0,
HeuristicEstimatePathLength = GetHeuristicPathLength(start, goal)
};
openSet.Add(startNode);
while (openSet.Count > 0)
{
// Шаг 3.
var currentNode = openSet.OrderBy(node =>
node.EstimateFullPathLength).First();
// Шаг 4.
if (currentNode.Position == goal /*|| GetHeuristicPathLength(currentNode.Position, start) > 50*/)
{
return GetPathForNode(currentNode);
}
// Шаг 5.
openSet.Remove(currentNode);
closedSet.Add(currentNode);
// Шаг 6.
foreach (var neighbourNode in GetNeighbours(currentNode, goal, field))
{
// Шаг 7.
if (closedSet.Count(node => node.Position == neighbourNode.Position) > 0)
{
continue;
}
var openNode = openSet.FirstOrDefault(node =>
node.Position == neighbourNode.Position);
// Шаг 8.
if (openNode == null)
openSet.Add(neighbourNode);
else
if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
{
// Шаг 9.
openNode.CameFrom = currentNode;
openNode.PathLengthFromStart = neighbourNode.PathLengthFromStart;
}
}
}
// Шаг 10.
return null;
}
public PathNode(V1 vertex, PathNode <V1> parent) : this(vertex)
{
Parent = parent;
}
private void OnCellChange(Vector2Int cellPosition, PathNode value)
{
updateMesh = true;
}
private int GetHeuristic(int x0, int y0, PathNode endNode) => COST_STRAIGHT_MOVE * (Mathf.Abs(x0 - endNode.x) + Mathf.Abs(y0 - endNode.z));
private NativeList <int2> CalculatePath(NativeArray <PathNode> pathNodeArray, PathNode endNode)
{
if (endNode.cameFronNodeIndex == -1)
{
// Couldn't find a path
return(new NativeList <int2>(Allocator.Temp));
}
else
{
// Found a path
NativeList <int2> path = new NativeList <int2>(Allocator.Temp);
path.Add(new int2(endNode.x, endNode.y));
PathNode currentNode = endNode;
while (currentNode.cameFronNodeIndex != -1)
{
PathNode cameFromNode = pathNodeArray[currentNode.cameFronNodeIndex];
path.Add(new int2(cameFromNode.x, cameFromNode.y));
currentNode = cameFromNode;
}
return(path);
}
}
public List <Vector3> FindPath()
{
// the target position is not workable
if (!theMap[targetX, targetY].passable)
{
return(null);
}
// if no move needed
else if (startX == targetX && startY == targetY)
{
return(null);
}
theGrid = new PathNode[mapX, mapY];
for (int x = 0; x < mapX; x++)
{
for (int y = 0; y < mapY; y++)
{
theGrid[x, y] = new PathNode(x, y, theMap[x, y].passable);
theGrid[x, y].cameFromNode = null;
theGrid[x, y].gCost = 99999999;
theGrid[x, y].FCost();
}
}
PathNode startNode = theGrid[startX, startY];
PathNode endNode = theGrid[targetX, targetY];
startNode.gCost = 0;
startNode.hCost = CalculateHCost(startNode, endNode);
startNode.FCost();
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
while (openList.Count > 0)
{
PathNode currentNode = FindMinFCostNode(openList);
// if we reached the target node
if (currentNode == endNode)
{
return(FormPath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode node in FindNeighbourNode(currentNode))
{
if (!node.isWalkable)
{
closedList.Add(node);
continue;
}
int currGCost = currentNode.gCost + CalculateHCost(currentNode, node);
if (currGCost < node.gCost)
{
node.cameFromNode = currentNode;
node.gCost = currGCost;
node.hCost = CalculateHCost(node, endNode);
node.FCost();
if (!openList.Contains(node))
{
openList.Add(node);
}
}
}
}
// when openList is empty; therefore, no path found
return(null);
}
public int Compare(PathNode <V> x, PathNode <V> y)
{
return(ComparePathNodes(x, y));
}
void FindNextPath(PathNode start, NavTriangle endNode, List <PathNode> openList, List <PathNode> closeList)
{
for (int i = 0; i < 3; i++)
{
if (start == null)
{
Debug.Log("是空的");
}
if (start.node.nodeArr[i] == null || IsCloseListContains(start.node.nodeArr[i]))
{
continue;
}
float g = start.dis + start.node.dis[i];
float h = Vector3.Distance(start.node.nodeArr[i].center, endNode.center);
float f = g + h;
PathNode path = new PathNode();
path.node = start.node.nodeArr[i];
path.dis = f;
path.parent = start;
openList.Add(path);
}
//取最小F节点，加入闭合列表
for (int i = 0; i < openList.Count; i++)
{
for (int j = i + 1; j < openList.Count; j++)
{
if (openList[i].dis > openList[j].dis)
{
PathNode path = openList[i];
openList[i] = openList[j];
openList[j] = path;
}
}
}
//如果最小路径是错误的路径，则标记错误。
PathNode curPath = null;
if (openList.Count > 0)
{
curPath = openList[0];
openList.RemoveAt(0);
closeList.Add(curPath);
//将结束点加入闭合列表中
if (openList == null)
{
Debug.Log("是空的");
}
if (curPath.node.center == endNode.center)
{
//PathNode path = new PathNode();
//path.node = endNode;
//path.parent = curPath;
//closeList.Add(path);
return;
}
}
FindNextPath(curPath, endNode, openList, closeList);
}
public List <PathNode> FindCenterPointPath(Vector3 start, Vector3 end)
{
//标记开始结束点
startVertex = FindNodeByPoint(start);
endVertex = FindNodeByPoint(end);
if (startVertex == null && endVertex == null)
{
return(null);
}
else if (startVertex == null)
{
startVertex = FindNearestNode(start);
}
else if (endVertex == null)
{
endVertex = FindNearestNode(end);
}
PathNode startNode = new PathNode();
startNode.node = startVertex;
PathNode endNode = new PathNode();
endNode.node = endVertex;
//如果起点和终点在同一个三角形中，则返回一个节点
if (startVertex.center == endVertex.center)
{
pathList.Clear();
pathList.Add(startNode);
return(pathList);
}
//如果起点和终点在两个相邻的三角形中，则返回二个节点
else if (TriangleUtil.GetTwoPointsInTwoPath(startNode, endNode) != null)
{
pathList.Clear();
pathList.Add(startNode);
pathList.Add(endNode);
return(pathList);
}
List <PathNode> openList = new List <PathNode>();
closeList = new List <PathNode>();
PathNode path = new PathNode();
path.node = startVertex;
path.dis = 0;
closeList.Add(path);
FindNextPath(path, endVertex, openList, closeList);
//记录并显示路径
if (showProcessPath)
{
ShowProcessPath();
}
else
{
ShowCenterPath();
}
return(pathList);
}
public Tuple(uint f, PathNode node)
{
this.F = f;
this.node = node;
}
/** Returns the node with the lowest F score from the heap */
public PathNode Remove()
{
PathNode returnItem = heap[0].node;
#if DECREASE_KEY
returnItem.heapIndex = NotInHeap;
#endif
numberOfItems--;
if (numberOfItems == 0)
{
return(returnItem);
}
// Last item in the heap array
var swapItem = heap[numberOfItems];
var swapItemG = swapItem.node.G;
int swapIndex = 0, parent;
// Trickle upwards
while (true)
{
parent = swapIndex;
uint swapF = swapItem.F;
int pd = parent * D + 1;
// If this holds, then the indices used
// below are guaranteed to not throw an index out of bounds
// exception since we choose the size of the array in that way
if (pd <= numberOfItems)
{
// Loading all F scores here instead of inside the if statements
// reduces data dependencies and improves performance
uint f0 = heap[pd + 0].F;
uint f1 = heap[pd + 1].F;
uint f2 = heap[pd + 2].F;
uint f3 = heap[pd + 3].F;
// The common case is that all children of a node are present
// so the first comparison in each if statement below
// will be extremely well predicted so it is essentially free
// (I tried optimizing for the common case, but it didn't affect performance at all
// at the expense of longer code, the CPU branch predictor is really good)
if (pd + 0 < numberOfItems && (f0 < swapF || (SortGScores && f0 == swapF && heap[pd + 0].node.G < swapItemG)))
{
swapF = f0;
swapIndex = pd + 0;
}
if (pd + 1 < numberOfItems && (f1 < swapF || (SortGScores && f1 == swapF && heap[pd + 1].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G))))
{
swapF = f1;
swapIndex = pd + 1;
}
if (pd + 2 < numberOfItems && (f2 < swapF || (SortGScores && f2 == swapF && heap[pd + 2].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G))))
{
swapF = f2;
swapIndex = pd + 2;
}
if (pd + 3 < numberOfItems && (f3 < swapF || (SortGScores && f3 == swapF && heap[pd + 3].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G))))
{
swapIndex = pd + 3;
}
}
// One if the parent's children are smaller or equal, swap them
// (actually we are just pretenting we swapped them, we hold the swapData
// in local variable and only assign it once we know the final index)
if (parent != swapIndex)
{
heap[parent] = heap[swapIndex];
#if DECREASE_KEY
heap[parent].node.heapIndex = (ushort)parent;
#endif
}
else
{
break;
}
}
// Assign element to the final position
heap[swapIndex] = swapItem;
#if DECREASE_KEY
swapItem.node.heapIndex = (ushort)swapIndex;
#endif
// For debugging
// Validate ();
return(returnItem);
}
public void Execute()
{
int2 gridSize = new int2(20, 20);
NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
PathNode pathNode = new PathNode();
pathNode.x = x;
pathNode.y = y;
pathNode.index = CalculateIndex(x, y, gridSize.x);
pathNode.gCost = int.MaxValue;
pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
pathNode.CalculateFCost();
pathNode.isWalkable = true;
pathNode.cameFronNodeIndex = -1;
pathNodeArray[pathNode.index] = pathNode;
}
}
// place walls
// {
// PathNode walkablePathNode = pathNodeArray[CalculateIndex(1, 0, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 0, gridSize.x)] = walkablePathNode;
// walkablePathNode = pathNodeArray[CalculateIndex(1, 1, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 1, gridSize.x)] = walkablePathNode;
// walkablePathNode = pathNodeArray[CalculateIndex(1, 2, gridSize.x)];
// walkablePathNode.SetIsWalkable(false);
// pathNodeArray[CalculateIndex(1, 2, gridSize.x)] = walkablePathNode;
// }
NativeArray <int2> neighbourOffsetsArray = new NativeArray <int2>(8, Allocator.Temp);
neighbourOffsetsArray[0] = new int2(-1, 0); // left
neighbourOffsetsArray[1] = new int2(+1, 0); // right
neighbourOffsetsArray[2] = new int2(0, +1); // up
neighbourOffsetsArray[3] = new int2(0, -1); // down
neighbourOffsetsArray[4] = new int2(-1, -1); // left down
neighbourOffsetsArray[5] = new int2(-1, +1); // left up
neighbourOffsetsArray[6] = new int2(+1, -1); // right down
neighbourOffsetsArray[7] = new int2(+1, +1); // right up
int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startingPosition.x, startingPosition.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFCost();
pathNodeArray[startNode.index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.index);
while (openList.Length > 0)
{
int currrentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currrentNodeIndex];
if (currrentNodeIndex == endNodeIndex)
{
// reached dest
break;
}
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currrentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currrentNodeIndex);
for (int i = 0; i < neighbourOffsetsArray.Length; i++)
{
int2 neighbourOffset = neighbourOffsetsArray[i];
int2 neightbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
if (!IsPositionInsideGrid(neightbourPosition, gridSize))
{
// Neighbour not valid position
continue;
}
int neightbourNodeIndex = CalculateIndex(neightbourPosition.x, neightbourPosition.y, gridSize.x);
if (closedList.Contains(neightbourNodeIndex))
{
// Allready searched this one
continue;
}
PathNode neightbourNode = pathNodeArray[neightbourNodeIndex];
if (!neightbourNode.isWalkable)
{
// Not walkable
continue;
}
int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neightbourPosition);
if (tentativeGCost < neightbourNode.gCost)
{
neightbourNode.cameFronNodeIndex = currrentNodeIndex;
neightbourNode.gCost = tentativeGCost;
neightbourNode.CalculateFCost();
pathNodeArray[neightbourNodeIndex] = neightbourNode;
if (!openList.Contains(neightbourNode.index))
{
openList.Add(neightbourNode.index);
}
}
}
}
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFronNodeIndex != -1)
{
// Found a path
NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
// foreach (int2 pathPosition in path) {
// Debug.Log(pathPosition);
// }
path.Dispose();
}
pathNodeArray.Dispose();
neighbourOffsetsArray.Dispose();
openList.Dispose();
closedList.Dispose();
}
protected virtual Double Heuristic(PathNode inStart, PathNode inEnd)
{
return(Math.Sqrt((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y)));
}
protected static int ComparePathNodes(PathNode <V> x, PathNode <V> y)
{
return(Math.Sign(x.Weight - y.Weight));
}
public void SetParent(PathNode p)
{
this.ParentNode = p;
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = m_grid.GetGridObject(startX, startY);
PathNode endNode = m_grid.GetGridObject(endX, endY);
if (startNode == null || endNode == null)
{
return(null); // Invalid path
}
m_openList = new List <PathNode> {
startNode
};
m_closeList = new List <PathNode>();
for (int x = 0; x < m_grid.GetWidth(); x++)
{
for (int y = 0; y < m_grid.GetHeight(); y++)
{
PathNode pathNode = m_grid.GetGridObject(x, y);
pathNode.SetGCost(int.MaxValue);
pathNode.CalculateFCost();
pathNode.SetPreviousNode(null);
}
}
startNode.SetGCost(0);
startNode.SetHCost(CalculateDistanceCost(startNode, endNode));
startNode.CalculateFCost();
while (m_openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(m_openList);
// Reached final node
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
m_openList.Remove(currentNode);
m_closeList.Add(currentNode);
foreach (PathNode neighbouringNode in GetNeighbourList(currentNode))
{
if (m_closeList.Contains(neighbouringNode))
{
continue;
}
if (!neighbouringNode.isWalkable)
{
m_closeList.Add(neighbouringNode);
continue;
}
int tentativeGCost = currentNode.GetGCost() + CalculateDistanceCost(currentNode, neighbouringNode);
if (tentativeGCost < neighbouringNode.GetGCost())
{
neighbouringNode.SetPreviousNode(currentNode);
neighbouringNode.SetGCost(tentativeGCost);
neighbouringNode.SetHCost(CalculateDistanceCost(neighbouringNode, endNode));
neighbouringNode.CalculateFCost();
if (!m_openList.Contains(neighbouringNode))
{
m_openList.Add(neighbouringNode);
}
}
}
}
// Ran out of nodes on the m_openList
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = _grid.GetGridObject(startX, startY);
PathNode endNode = _grid.GetGridObject(endX, endY);
if (startNode == null || endNode == null)
{
return(null);
}
_openList = new List <PathNode> {
startNode
};
_closedList = new List <PathNode>();
for (int x = 0; x < _grid.GetWidth(); x++)
{
for (int y = 0; y < _grid.GetHeight(); y++)
{
PathNode pathNode = _grid.GetGridObject(x, y);
pathNode.gCost = 99999999;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
PathfindingDebugStepVisual.Instance.ClearSnapshots();
PathfindingDebugStepVisual.Instance.TakeSnapshot(_grid, startNode, _openList, _closedList);
while (_openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(_openList);
if (currentNode == endNode)
{
PathfindingDebugStepVisual.Instance.TakeSnapshot(_grid, currentNode, _openList, _closedList);
PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(_grid, CalculatePath(endNode));
return(CalculatePath(endNode));
}
_openList.Remove(currentNode);
_closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (_closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
_closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!_openList.Contains(neighbourNode))
{
_openList.Add(neighbourNode);
}
}
PathfindingDebugStepVisual.Instance.TakeSnapshot(_grid, currentNode, _openList, _closedList);
}
}
return(null);
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
//Debug.Log("FindPath: start" + startX + "," + startY + " end" + endX + "," + endY);
PathNode startNode = GetNode(startX, startY);
PathNode endNode = GetNode(endX, endY);
if (startNode == null || endNode == null)
{
// Invalid Path
Debug.LogWarning("Invalid Path!");
return(null);
}
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
// Search
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if ((currentNode.Vector2Int() - endNode.Vector2Int()).magnitude < 15)
{
// Reached final node
return(CalculatePath(currentNode)); //not endnode because not exact hit
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
/*if (!neighbourNode.isWalkable) {
* closedList.Add(neighbourNode);
* continue;
* }*/
float tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode) + InclineCost(currentNode, neighbourNode) + WaterCost(neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// Out of nodes on the openList
Debug.Log("Failed to find path to " + endNode);
return(null);
}
internal PathNode(Node value, PathNode <Node>?next = null)
{
Value = value;
Next = next;
}
private void StoreNeighborNodes(PathNode inAround, PathNode[] inNeighbors)
{
//disabled diagonal move - lupusa 8/14/2018
int x = inAround.X;
int y = inAround.Y;
//if ((x > 0) && (y > 0))
// inNeighbors[0] = m_SearchSpace[x - 1, y - 1];
//else
// inNeighbors[0] = null;
if (y > 0)
{
inNeighbors[1] = m_SearchSpace[x, y - 1];
}
else
{
inNeighbors[1] = null;
}
//if ((x < Width - 1) && (y > 0))
// inNeighbors[2] = m_SearchSpace[x + 1, y - 1];
//else
// inNeighbors[2] = null;
if (x > 0)
{
inNeighbors[3] = m_SearchSpace[x - 1, y];
}
else
{
inNeighbors[3] = null;
}
if (x < Width - 1)
{
inNeighbors[4] = m_SearchSpace[x + 1, y];
}
else
{
inNeighbors[4] = null;
}
//if ((x > 0) && (y < Height - 1))
// inNeighbors[5] = m_SearchSpace[x - 1, y + 1];
//else
// inNeighbors[5] = null;
if (y < Height - 1)
{
inNeighbors[6] = m_SearchSpace[x, y + 1];
}
else
{
inNeighbors[6] = null;
}
//if ((x < Width - 1) && (y < Height - 1))
// inNeighbors[7] = m_SearchSpace[x + 1, y + 1];
//else
// inNeighbors[7] = null;
}
//private List<Int64> elapsed = new List<long>();
/// <summary>
/// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
/// is passed to IsWalkable().
/// </summary>
public LinkedList <TPathNode> Search(System.Drawing.Point inStartNode, System.Drawing.Point inEndNode, TUserContext inUserContext)
{
PathNode startNode = m_SearchSpace[inStartNode.X, inStartNode.Y];
PathNode endNode = m_SearchSpace[inEndNode.X, inEndNode.Y];
//System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
//watch.Start();
if (startNode == endNode)
{
return(new LinkedList <TPathNode>(new TPathNode[] { startNode.UserContext }));
}
PathNode[] neighborNodes = new PathNode[8];
m_ClosedSet.Clear();
m_OpenSet.Clear();
m_RuntimeGrid.Clear();
m_OrderedOpenSet.Clear();
for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
m_CameFrom[x, y] = null;
}
}
startNode.G = 0;
startNode.H = Heuristic(startNode, endNode);
startNode.F = startNode.H;
m_OpenSet.Add(startNode);
m_OrderedOpenSet.Push(startNode);
m_RuntimeGrid.Add(startNode);
int nodes = 0;
while (!m_OpenSet.IsEmpty)
{
PathNode x = m_OrderedOpenSet.Pop();
if (x == endNode)
{
// watch.Stop();
//elapsed.Add(watch.ElapsedMilliseconds);
LinkedList <TPathNode> result = ReconstructPath(m_CameFrom, m_CameFrom[endNode.X, endNode.Y]);
result.AddLast(endNode.UserContext);
return(result);
}
m_OpenSet.Remove(x);
m_ClosedSet.Add(x);
StoreNeighborNodes(x, neighborNodes);
for (int i = 0; i < neighborNodes.Length; i++)
{
PathNode y = neighborNodes[i];
Boolean tentative_is_better;
if (y == null)
{
continue;
}
if (!y.UserContext.IsWalkable(inUserContext))
{
continue;
}
if (m_ClosedSet.Contains(y))
{
continue;
}
nodes++;
Double tentative_g_score = m_RuntimeGrid[x].G + NeighborDistance(x, y);
Boolean wasAdded = false;
if (!m_OpenSet.Contains(y))
{
m_OpenSet.Add(y);
tentative_is_better = true;
wasAdded = true;
}
else if (tentative_g_score < m_RuntimeGrid[y].G)
{
tentative_is_better = true;
}
else
{
tentative_is_better = false;
}
if (tentative_is_better)
{
m_CameFrom[y.X, y.Y] = x;
if (!m_RuntimeGrid.Contains(y))
{
m_RuntimeGrid.Add(y);
}
m_RuntimeGrid[y].G = tentative_g_score;
m_RuntimeGrid[y].H = Heuristic(y, endNode);
m_RuntimeGrid[y].F = m_RuntimeGrid[y].G + m_RuntimeGrid[y].H;
if (wasAdded)
{
m_OrderedOpenSet.Push(y);
}
else
{
m_OrderedOpenSet.Update(y);
}
}
}
}
return(null);
}
private float InclineCost(PathNode a, PathNode b)
{
return(5 * Mathf.Abs(a.height - b.height));
}
public PathNode MoveToNextNode(NPC currentNPC)
{
PathNode nextNode = this;
if (currentNPC.GetCurrentPathArray() != null)
{
PathNode[] currentPath = unshiftPaths(currentNPC.GetCurrentPathArray());
currentNPC.SetCurrentPath(currentPath);
//cycle through the paths to find the next valid one and move to it
nextNode = GetDestinationNode(currentPath);
if (nextNode.type == "Exit" && !currentNPC.hasSanityLeft())
{
currentNPC.MakeNPCRunOutOfHouse();
}
else if (nextNode.type == "Door")
{
PathNode nodeDoorOfNextRoom = nextNode.nodeToOtherRoom.GetComponent <PathNode>();
nextNode = nodeDoorOfNextRoom;
currentNPC.SetMovedToNewRoom();
}
if (nextNode != null)
{
nextNode.OccupyNode(this, currentNPC);
}
}
else
{
bool movePlayerToDoor = currentNPC.wantsToLeaveRoom();
if (!currentNPC.hasSanityLeft())
{
PathNode[] route = { this };
checkNeighbourNodes(route, this, "Exit");
if (_exitRoute != null)
{
currentNPC.SetCurrentPath(_exitRoute);
nextNode = MoveToNextNode(currentNPC);
}
else
{
//Wants to exit but no exit in this room, so move em to another room
movePlayerToDoor = true;
}
//player really wants to leave house
//check if an exit door exists and map it
}
if (movePlayerToDoor)
{
//TODO: The generated exit path never changes for the node, maybe just call it once?
PathNode[] route = { this };
if (this.type == "Door")
{
currentNPC.SetCurrentPath(route);
nextNode = MoveToNextNode(currentNPC);
}
else
{
if (_currentFastestRoute == null)
{
checkNeighbourNodes(route, this, "Door");
}
if (_currentFastestRoute != null)
{
currentNPC.SetCurrentPath(_currentFastestRoute);
nextNode = MoveToNextNode(currentNPC);
}
else
{
print("ERROR!: " + currentNPC.npcName + " wants to change rooms and route is " + _currentFastestRoute);
}
}
}
else
{
nextNode = MoveToRandomNode(currentNPC);
}
}
return(nextNode);
}
public static List <PathNode> GetTilesAsList(Dictionary <Tile.Sides, Tile> tiles, PathNode baseNode, bool IsWalkable, Tile destination, List <PathNode> NotInthisList, Grid gameGrid)
{
List <PathNode> pnTiles = new List <PathNode>();
foreach (KeyValuePair <Tile.Sides, Tile> t in tiles)
{
Tile nTile = t.Value;
if (nTile != null && (nTile.IsWalkable == IsWalkable) && !NotInthisList.Exists(x => x.GetTile() == nTile))
{
PathNode pn = new PathNode(t.Value, baseNode);
pnTiles.Add(pn);
if (IsWalkable == true)
{
pn.SetG(pn.GetParent().GetG() + pn.GetTile().MovementCost);
pn.SetH(GetDistanceToNode(pn.GetTile(), destination, gameGrid));
//Debug.Log(" NodeID: " + baseNode.GetTile().Id + ", LookingID: " + pn.GetTile().Id + ", G: " + pn.GetG() + ", H: " + pn.GetH() + ", F: " + pn.GetF());
}
}
}
return(pnTiles);
}
public abstract bool isAtEnd(PathNode currentNode, Point endPoint, GameLocation location, Character c);
void Map_MethodInvoke(PathNode node)
{
try {
node.value = FastWrapper.MethodInvoke(node.model.GetType(), node.key, node.model, new object[0]);
} catch { }
}
public PathNode(Tile tile, PathNode parent) : this(tile, parent, 0, 0)
{
}
//removes connection to another node, should be called by rotating nodes that are no longer connected to this one
void RemoveConnection(PathNode toRemove)
{
connected.Remove(toRemove);
}
public PathNode(V1 vertex, PathNode <V1> parent, NodeSearchStatus status) : this(vertex, parent)
{
Status = status;
}
public List <PathNode> FindPath(int startX, int startY, int endX, int endY)
{
PathNode startNode = grid.GetGridObject(startX, startY);
PathNode endNode = grid.GetGridObject(endX, endY);
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
for (int x = 0; x < grid.GetWidth(); x++)
{
for (int y = 0; y < grid.GetHeight(); y++)
{
PathNode pathNode = grid.GetGridObject(x, y);
pathNode.gCost = int.MaxValue;
pathNode.CalculateFCost();
pathNode.cameFromNode = null;
}
}
startNode.gCost = 0;
startNode.hCost = CalculateDistanceCost(startNode, endNode);
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode == endNode)// reached final node;
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.isWalkable)
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode);
if (tentativeGCost < neighbourNode.gCost)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.gCost = tentativeGCost;
neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// out of nodes on openlist searched all nodes doesnt find path
return(null);
}
public Task <PathResult> GetGraphPath(PathNode start, PathNode stop)
{
return(Task.Factory.StartNew(() => CalculatePath(start, stop)));
}
