protected override void OnUpdate()
{
var mapSettings = GetSingleton <MapSettings>();
var neighbors = _neighbors;
// The data is in not valid state
if (mapSettings.Tiles.Length < 1)
{
return;
}
// Other data
var movementComponents = GetComponentDataFromEntity <MovementCost>(true);
var tilesSize = mapSettings.Tiles.Length;
var commandBuffer = _eseCommandBufferSystem.CreateCommandBuffer();
Entities
.WithoutBurst()
.WithNone <Waypoint>()
.ForEach((Entity requestEntity, ref PathRequest pathRequest) =>
{
_markerAStar.Begin();
#region Setup
_markerSetup.Begin();
NativeArray <float2> costs = new NativeArray <float2>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <byte> closeSet = new NativeArray <byte>(tilesSize, Allocator.Temp);
NativeArray <int> camesFrom = new NativeArray <int>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeMinHeap minSet = new NativeMinHeap(tilesSize * 2, Allocator.Temp);
_markerSetupData.Begin();
// Setup initial data
for (int i = 0; i < tilesSize; i++)
{
costs[i] = new float2 {
x = 0, y = float.MaxValue
};
camesFrom[i] = -1;
}
_markerSetupData.End();
// Shortcuts
int startTile = Index1D(pathRequest.Start, tilesSize);
int endTile = Index1D(pathRequest.End, tilesSize);
costs[startTile] = 0;
_markerSetup.End();
#endregion Setup
#region Search
_markerSearch.Begin();
int lastTile = startTile;
// While not in destination and not at dead end
while (lastTile != endTile && lastTile != -1)
{
// Mark current tile as visited
closeSet[lastTile] = 1;
for (int i = 0; i < neighbors.Length; ++i)
{
var neighbor = neighbors[i];
// Find linear neighbor index
var neighborIndex = neighbor.Of(lastTile, tilesSize);
// Check if neighbor exists
if (neighborIndex != -1)
{
// Previous + current cost
_markerMovementData.Begin();
var currentCost = movementComponents[mapSettings.Tiles[neighborIndex]];
_markerMovementData.End();
if (currentCost.Cost == MovementCost.IMPOSSIBLE)
{
closeSet[neighborIndex] = 0;
continue;
}
var costG = costs[lastTile].x + neighbor.Distance * currentCost.Cost;
var costF = costG + Implementation.Heuristic(pathRequest, neighborIndex, tilesSize);
if (costs[neighborIndex].y > costF)
{
// Update cost and path
costs[neighborIndex] = new float2(costG, costF);
camesFrom[neighborIndex] = lastTile;
// Update min set
if (closeSet[neighborIndex] == 0)
{
minSet.Push(new MinHeapNode(neighborIndex, costF));
}
}
}
}
lastTile = Implementation.FindCurrent(minSet, closeSet);
}
_markerSearch.End();
#endregion Search
#region ReconstructPath
_markerReconstruct.Begin();
var waypoints = commandBuffer.AddBuffer <Waypoint>(requestEntity);
// Travel back through path
while (lastTile != -1)
{
waypoints.Add(new Waypoint()
{
Position = Index2D(lastTile, tilesSize)
});
lastTile = camesFrom[lastTile];
}
_markerReconstruct.End();
#endregion ReconstructPath
commandBuffer.RemoveComponent <PathRequest>(requestEntity);
#region Cleanup
_markerCleanup.Begin();
// Dispose all temporary data
costs.Dispose();
closeSet.Dispose();
camesFrom.Dispose();
minSet.Dispose();
_markerCleanup.End();
#endregion Cleanup
_markerAStar.End();
}).Run();
}
public void Execute()
{
int length = moveCost.Length;
int start1d = Index2dTo1d(start, moveCost_width);
{
for (int i = _F_.Length - 1; i != -1; i--)
{
_F_[i] = float.MaxValue;
}
_F_[start1d] = 0;
}
{
solution[start1d] = start;
}
{
frontier.Push(start);
}
{
visited.TryAdd(start, 0);
}
//solve;
float euclideanMaxLength = EuclideanHeuristicMaxLength(length, moveCost_width);
int2 node = int2.zero - 1;
while (frontier.Count != 0 && math.any(node != destination))
{
node = frontier.Pop(); //we grab candidate with lowest F so far
int node1d = Index2dTo1d(node, moveCost_width);
float node_f = _F_[node1d];
//lets check all its neighbours:
EnumerateNeighbours(neighbours, moveCost_width, moveCost_width, node);
int neighboursLength = neighbours.Length;
for (int i = 0; i < neighboursLength; i++)
{
int2 neighbour = neighbours[i];
int neighbour1d = Index2dTo1d(neighbour, moveCost_width);
bool isOrthogonal = math.any(node == neighbour); //is relative position orthogonal or diagonal
float g = node_f + moveCost[neighbour1d] * (isOrthogonal ? 1f : 1.41421356237f);
float h = EuclideanHeuristicNormalized(neighbour, destination, euclideanMaxLength) * heuristic_cost;
float f = g + h;
//update F when this connection is less costly:
if (
f < _F_[neighbour1d]
)
{
_F_[neighbour1d] = f;
solution[neighbour1d] = node;
}
//update frontier:
if (visited.TryAdd(neighbour, 0))
{
frontier.Push(neighbour);
}
}
}
//create path:
BacktrackToPath(solution, moveCost_width, destination, Results);
}
private void FindPath(int2 startPosition, int2 endPosition)
{
Debug.Log("Pathfinding to: " + endPosition);
if (startPosition.Equals(endPosition) || Grid[GetIndex(endPosition)] == -1)
{
Debug.LogError("asking to go into an obstacle, or is already there!");
foundPath = false;
return;
}
PathNode head = new PathNode(startPosition, CalculateDistanceCost(startPosition, endPosition));
OpenSet.Push(head);
while (itterationLimit > 0 && OpenSet.HasNext())
{
int currentIndex = OpenSet.Pop();
PathNode current = OpenSet[currentIndex];
int ind = GetIndex(current.Position);
PathNode cameFromNode = CameFrom[ind];
if (current.Position.Equals(endPosition))
{
//Found our destination, we will let the cleanup job handle the path reconstruction for now
//ReconstructPath(startPosition, endPosition);
return;
}
float initialCost = CostSoFar[GetIndex(current.Position)];
PathNode[] neighbourNodes = new PathNode[Neighbours.Length];
for (int i = 0; i < Neighbours.Length; i++)
{
int2 neighbour = Neighbours[i];
int2 position = current.Position + neighbour;
if (position.x < 0 || position.x >= DimX || position.y < 0 || position.y >= DimY)
{
continue;
}
int index = GetIndex(position);
float cellCost = GetCellCost(currentIndex, index, true);
if (float.IsInfinity(cellCost))
{
current.NextToObstacle = true;
continue;
}
neighbourNodes[i] = new PathNode(position, cellCost);
}
if (!cameFromNode.Equals(null) && cameFromNode.NextToObstacle && current.NextToObstacle && IsDiagonal(current.Position, cameFromNode.Position))
{
//In this case, the path came from point that was next to a obstacle, and is moving diagonally towards a point next to an obstacle. so we are assuming they are moving diagonally through the obstacle
//TODO: this is not always the case, will need to resolve later
continue;
}
for (int i = 0; i < neighbourNodes.Length; i++)
{
int2 neighbour = Neighbours[i];
PathNode neighbourNode = neighbourNodes[i];
int index = GetIndex(neighbourNode.Position);
if (neighbourNode.Equals(null))
{
Debug.Log("neighbour null");
continue;
}
float neighbourCost = 10;
if ((math.abs(neighbour.x) + math.abs(neighbour.y)) == 2)
{
neighbourCost = 14;
}
float newCost = initialCost + neighbourCost + neighbourNode.ExpectedCost;
float oldCost = CostSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
CostSoFar[index] = newCost;
CameFrom[index] = current;
neighbourNode.ExpectedCost = newCost + CalculateDistanceCost(neighbourNode.Position, endPosition);
OpenSet.Push(neighbourNode);
}
itterationLimit--;
}
if (OpenSet.HasNext())
{
//We ran out of itterations
//We will just give out where we stapped at for now
//TODO: fix this
var currentIndex = OpenSet.Pop();
endPosition = OpenSet[currentIndex].Position;
}
}
public void Execute(Entity entity, int index, ref PathFindingRequest request)
{
_iterations = 0;
_pathNodeCount = 0;
//Generate Working Containers
var openSet = new NativeMinHeap(MapSize, Allocator.Temp);
var cameFrom = new NativeArray <int>(MapSize, Allocator.Temp);
var costCount = new NativeArray <int>(MapSize, Allocator.Temp);
for (var i = 0; i < MapSize; i++)
{
costCount[i] = int.MaxValue;
}
// Path finding
var startId = request.StartId;
var goalId = request.GoalId;
openSet.Push(new MinHeapNode(startId, 0));
costCount[startId] = 0;
var currentId = -1;
while (_iterations < IterationLimit && openSet.HasNext())
{
var currentNode = openSet[openSet.Pop()];
currentId = currentNode.Id;
if (currentId == goalId)
{
break;
}
var neighboursId = new NativeList <int>(4, Allocator.Temp);
Nodes[currentId].GetNeighbours(ref neighboursId);
foreach (var neighbourId in neighboursId)
{
//if cost == -1 means obstacle, skip
if (Nodes[neighbourId].GetCost() == -1)
{
continue;
}
var currentCost = costCount[currentId] == int.MaxValue
? 0
: costCount[currentId];
var newCost = currentCost + Nodes[neighbourId].GetCost();
//not better, skip
if (costCount[neighbourId] <= newCost)
{
continue;
}
var priority = newCost + Nodes[neighbourId].Heuristic(goalId);
openSet.Push(new MinHeapNode(neighbourId, priority));
cameFrom[neighbourId] = currentId;
costCount[neighbourId] = newCost;
}
_iterations++;
neighboursId.Dispose();
}
//Construct path
var buffer = ResultECB.AddBuffer <PathRoute>(index, entity);
var nodeId = goalId;
while (_pathNodeCount < PathNodeLimit && !nodeId.Equals(startId))
{
buffer.Add(new PathRoute {
Id = nodeId
});
nodeId = cameFrom[nodeId];
_pathNodeCount++;
}
//Construct Result
var success = true;
var log = new NativeString64("Path finding success");
if (!openSet.HasNext() && currentId != goalId)
{
success = false;
log = new NativeString64("Out of openset");
}
if (_iterations >= IterationLimit && currentId != goalId)
{
success = false;
log = new NativeString64("Iteration limit reached");
}
else if (_pathNodeCount >= PathNodeLimit && !nodeId.Equals(startId))
{
success = false;
log = new NativeString64("Step limit reached");
}
ResultECB.AddComponent(index, entity, new PathResult
{
Success = success,
Log = log
});
//Clean result at end of simulation
CleanECB.DestroyEntity(index, entity);
//Clear
openSet.Dispose();
cameFrom.Dispose();
costCount.Dispose();
}
private void FindPathUsingAStar(int startNavUnit, int endNavUnit, NativeList <int> path)
{
if (startNavUnit < 0 || endNavUnit < 0 ||
!navUnits[startNavUnit].IsNavigable() || !navUnits[endNavUnit].IsNavigable() ||
startNavUnit == endNavUnit)
{
return;
}
Bounds endNavUnitBounds = navUnits[endNavUnit].GetRelativeBounds();
// SimplePriorityQueue<int> openList = new SimplePriorityQueue<int>();
// HashSet<int> closedList = new HashSet<int>();
NativeMinHeap <int> openList = new NativeMinHeap <int>(Allocator.Temp);
NativeArray <bool> closedList = new NativeArray <bool>(navUnits.Length, Allocator.Temp);
int l = 0;
for (int j = 0; j < navGridSizeY; j++)
{
for (int k = 0; k < navGridSizeZ; k++)
{
for (int i = 0; i < navGridSizeX; i++)
{
navUnits[l] = navUnits[l].ResetPathFindingData();
l++;
}
}
}
openList.Push(startNavUnit, navUnits[startNavUnit].AStarData.F);
while (openList.Size() > 0)
{
int curNavUnit = openList.Pop();
int3 curNavPos = GetPosFromIndex(curNavUnit);
Bounds curNavUnitBounds = navUnits[curNavUnit].GetRelativeBounds();
for (int i = 0; i < neighborOffsets.Length; i++)
{
int3 neighborOffset = neighborOffsets[i];
int neighbor = GetIndexFromPos(curNavPos.x + neighborOffset.x, curNavPos.y + neighborOffset.y,
curNavPos.z + neighborOffset.z);
if (neighbor < 0 || !navUnits[neighbor].IsNavigable())
{
continue;
}
if (neighbor == endNavUnit)
{
navUnits[neighbor] = navUnits[neighbor].SetPathFindingParentIndex(curNavUnit);
BackTracePath(endNavUnit, path);
return;
}
if (!closedList[neighbor])
{
Bounds neighborBounds = navUnits[neighbor].GetRelativeBounds();
float newG = navUnits[curNavUnit].AStarData.G +
Vector3.Distance(curNavUnitBounds.center, neighborBounds.center);
float newH = Vector3.Distance(neighborBounds.center, endNavUnitBounds.center);
float newF = newG + newH;
if (newF < navUnits[neighbor].AStarData.F)
{
navUnits[neighbor] =
navUnits[neighbor].UpdatePathFindingValues(newF, newG, newH, curNavUnit);
openList.Push(neighbor, navUnits[neighbor].AStarData.F);
}
}
}
closedList[curNavUnit] = true;
}
openList.Dispose();
closedList.Dispose();
}
