123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275 |
- using System.Collections;
- using System.Collections.Generic;
- using UnityEngine;
- using Unity.Mathematics;
- using Unity.Collections;
- using Unity.Jobs;
- using Unity.Burst;
- using KairoEngine.Core;
- public class GridPathfinding : MonoBehaviour {
- private const int MOVE_STRAIGHT_COST = 10;
- private const int MOVE_DIAGONAL_COST = 14;
- private int2 gridSize;
- private NativeArray<int> mapArray;
- private void Start() {
- gridSize = new int2(32, 32);
- mapArray = new NativeArray<int>(gridSize.x * gridSize.y, Allocator.Persistent);
- for (int x = 0; x < gridSize.x; x++) {
- for (int y = 0; y < gridSize.y; y++) {
- mapArray[x + y * gridSize.x] = 1;
- }
- }
- Timer.ExecuteRealTime(3000, () => {Loop(); });
- }
- private void OnDestroy()
- {
- mapArray.Dispose();
- }
- private void Loop()
- {
- Timer.ExecuteRealTime(2000, () => {
- float startTime = Time.realtimeSinceStartup;
- int findPathJobCount = 200;
- NativeArray<JobHandle> jobHandleArray = new NativeArray<JobHandle>(findPathJobCount, Allocator.TempJob);
- List<FindPathJob> jobsArray = new List<FindPathJob>();
- int2 startPosition = new int2(0, 0);
- int2 endPosition = new int2(31, 31);
- for (int i = 0; i < findPathJobCount; i++) {
- FindPathJob findPathJob = new FindPathJob {
- startPosition = startPosition,
- endPosition = endPosition,
- gridSize = gridSize,
- pathFound = new NativeArray<int>(2, Allocator.TempJob),
- mapArray = mapArray
- };
- jobHandleArray[i] = findPathJob.Schedule();
- jobsArray.Add(findPathJob);
- }
- JobHandle.CompleteAll(jobHandleArray);
- for (int i = 0; i < jobsArray.Count; i++)
- {
- bool pathFound = jobsArray[i].pathFound[0] == 1 ? true : false;
- Debug.Log($"Path found: {pathFound} - {jobsArray[i].pathFound[1]} points");
- jobsArray[i].pathFound.Dispose();
- }
- jobHandleArray.Dispose();
- jobsArray.Clear();
- Debug.Log("Time: " + ((Time.realtimeSinceStartup - startTime) * 1000f));
- Loop();
- });
- }
- [BurstCompile]
- public struct FindPathJob : IJob {
- public int2 startPosition;
- public int2 endPosition;
- public int2 gridSize;
- public NativeArray<int> pathFound;
- [ReadOnly] public NativeArray<int> mapArray;
- public void Execute() {
- 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++) {
- int index = CalculateIndex(x, y, gridSize.x);
- PathNode pathNode = new PathNode();
- pathNode.x = x;
- pathNode.y = y;
- pathNode.index = index;
- pathNode.gCost = int.MaxValue;
- pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition);
- pathNode.CalculateFCost();
- pathNode.isWalkable = mapArray[index] == 1 ? true : false;
- pathNode.cameFromNodeIndex = -1;
- pathNodeArray[pathNode.index] = pathNode;
- }
- }
- NativeArray<int2> neighbourOffsetArray = new NativeArray<int2>(8, Allocator.Temp);
- neighbourOffsetArray[0] = new int2(-1, 0); // Left
- neighbourOffsetArray[1] = new int2(+1, 0); // Right
- neighbourOffsetArray[2] = new int2(0, +1); // Up
- neighbourOffsetArray[3] = new int2(0, -1); // Down
- neighbourOffsetArray[4] = new int2(-1, -1); // Left Down
- neighbourOffsetArray[5] = new int2(-1, +1); // Left Up
- neighbourOffsetArray[6] = new int2(+1, -1); // Right Down
- neighbourOffsetArray[7] = new int2(+1, +1); // Right Up
- int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x);
- PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.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 currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
- PathNode currentNode = pathNodeArray[currentNodeIndex];
- if (currentNodeIndex == endNodeIndex) {
- // Reached our destination!
- break;
- }
- // Remove current node from Open List
- for (int i = 0; i < openList.Length; i++) {
- if (openList[i] == currentNodeIndex) {
- openList.RemoveAtSwapBack(i);
- break;
- }
- }
- closedList.Add(currentNodeIndex);
- for (int i = 0; i < neighbourOffsetArray.Length; i++) {
- int2 neighbourOffset = neighbourOffsetArray[i];
- int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y);
- if (!IsPositionInsideGrid(neighbourPosition, gridSize)) {
- // Neighbour not valid position
- continue;
- }
- int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x);
- if (closedList.Contains(neighbourNodeIndex)) {
- // Already searched this node
- continue;
- }
- PathNode neighbourNode = pathNodeArray[neighbourNodeIndex];
- if (!neighbourNode.isWalkable) {
- // Not walkable
- continue;
- }
- int2 currentNodePosition = new int2(currentNode.x, currentNode.y);
- int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition);
- if (tentativeGCost < neighbourNode.gCost) {
- neighbourNode.cameFromNodeIndex = currentNodeIndex;
- neighbourNode.gCost = tentativeGCost;
- neighbourNode.CalculateFCost();
- pathNodeArray[neighbourNodeIndex] = neighbourNode;
- if (!openList.Contains(neighbourNode.index)) {
- openList.Add(neighbourNode.index);
- }
- }
- }
- }
- PathNode endNode = pathNodeArray[endNodeIndex];
- if (endNode.cameFromNodeIndex == -1) {
- // Didn't find a path!
- //Debug.Log("Didn't find a path!");
- pathFound[0] = 0;
- pathFound[1] = 0;
- } else {
- // Found a path
- pathFound[0] = 1;
- NativeList<int2> path = CalculatePath(pathNodeArray, endNode);
- pathFound[1] = path.Length;
- /*
- foreach (int2 pathPosition in path) {
- Debug.Log(pathPosition);
- }
- */
- path.Dispose();
- }
- //pathNodeArray.Dispose();
- neighbourOffsetArray.Dispose();
- openList.Dispose();
- closedList.Dispose();
- }
-
- public NativeList<int2> CalculatePath(NativeArray<PathNode> pathNodeArray, PathNode endNode) {
- if (endNode.cameFromNodeIndex == -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.cameFromNodeIndex != -1) {
- PathNode cameFromNode = pathNodeArray[currentNode.cameFromNodeIndex];
- path.Add(new int2(cameFromNode.x, cameFromNode.y));
- currentNode = cameFromNode;
- }
- return path;
- }
- }
- public bool IsPositionInsideGrid(int2 gridPosition, int2 gridSize) {
- return
- gridPosition.x >= 0 &&
- gridPosition.y >= 0 &&
- gridPosition.x < gridSize.x &&
- gridPosition.y < gridSize.y;
- }
- public int CalculateIndex(int x, int y, int gridWidth) {
- return x + y * gridWidth;
- }
- public int CalculateDistanceCost(int2 aPosition, int2 bPosition) {
- int xDistance = math.abs(aPosition.x - bPosition.x);
- int yDistance = math.abs(aPosition.y - bPosition.y);
- int remaining = math.abs(xDistance - yDistance);
- return MOVE_DIAGONAL_COST * math.min(xDistance, yDistance) + MOVE_STRAIGHT_COST * remaining;
- }
-
- public int GetLowestCostFNodeIndex(NativeList<int> openList, NativeArray<PathNode> pathNodeArray) {
- PathNode lowestCostPathNode = pathNodeArray[openList[0]];
- for (int i = 1; i < openList.Length; i++) {
- PathNode testPathNode = pathNodeArray[openList[i]];
- if (testPathNode.fCost < lowestCostPathNode.fCost) {
- lowestCostPathNode = testPathNode;
- }
- }
- return lowestCostPathNode.index;
- }
- }
- public struct PathNode {
- public int x;
- public int y;
- public int index;
- public int gCost;
- public int hCost;
- public int fCost;
- public bool isWalkable;
- public int cameFromNodeIndex;
- public void CalculateFCost() {
- fCost = gCost + hCost;
- }
- public void SetIsWalkable(bool isWalkable) {
- this.isWalkable = isWalkable;
- }
- }
- }
|