private BlockType IsBlocked(int x, int y, PathfindaxCollisionCategory collisionCategory, Point2 fromCoordinates)
{
if (x >= _definitionNodeGrid.DefinitionNodeArray.Width || y >= _definitionNodeGrid.DefinitionNodeArray.Height)
{
return(BlockType.Current);
}
var definitionNode = _definitionNodeGrid.DefinitionNodeArray[x, y];
var blockType = BlockType.None;
foreach (var nodeConnection in definitionNode.Connections)
{
if ((nodeConnection.CollisionCategory & collisionCategory) != 0)
{
var toNode = _definitionNodeGrid.DefinitionNodeArray[nodeConnection.To.Index];
var toCoordinates = new Point2((int)toNode.Position.X, (int)toNode.Position.Y);
if (toCoordinates.X >= fromCoordinates.X && toCoordinates.Y >= fromCoordinates.Y)
{
if (toCoordinates.X >= x || toCoordinates.Y >= y)
{
blockType = BlockType.Next;
}
else
{
return(BlockType.Current);
}
}
}
}
return(blockType);
}
public void Generate(AstarNode[] pathfindingNetwork, PathfindaxCollisionCategory collisionCategory)
{
for (var i = 0; i < _definitionNodeGrid.DefinitionNodeArray.Array.Length; i++)
{
ref var definitionNode = ref _definitionNodeGrid.DefinitionNodeArray.Array[i];
var clearance = CalculateClearance(ref definitionNode, collisionCategory, _maxClearance);
pathfindingNetwork[i].Clearance = clearance;
}
void ICmpInitializable.OnInit(InitContext context)
{
if (context == InitContext.Activate && DualityApp.ExecContext == DualityApp.ExecutionContext.Game)
{
_rigidBody = GameObj.GetComponent <RigidBody>();
_collisionCategory = (PathfindaxCollisionCategory)_rigidBody.CollisionCategory;
DualityApp.Mouse.ButtonDown += Mouse_ButtonDown;
}
}
void ICmpInitializable.OnInit(InitContext context)
{
if (context == InitContext.Activate && DualityApp.ExecContext == DualityApp.ExecutionContext.Game)
{
DynamicPotentialFieldComponent.PotentialField.AddPotentialFunction(this,
new QuadraticPotentialFunction(() => CurrentPosition, (float)AgentSize / 2 + 1.0f, 1.2f * AgentSize));
_rigidBody = GameObj.GetComponent <RigidBody>();
_collisionCategory = (PathfindaxCollisionCategory)_rigidBody.CollisionCategory;
DualityApp.Mouse.ButtonDown += Mouse_ButtonDown;
}
}
public DijkstraNode[] GetCollisionLayerNetwork(PathfindaxCollisionCategory collisionCategory)
{
if (!_nodeNetworks.TryGetValue(collisionCategory, out var pathfindingNetwork))
{
var watch = Stopwatch.StartNew();
pathfindingNetwork = GenerateNodeNetwork(collisionCategory);
Debug.WriteLine($"Generated pathfind nodenetwork in {watch.ElapsedMilliseconds} ms");
_nodeNetworks.Add(collisionCategory, pathfindingNetwork);
}
return(pathfindingNetwork);
}
public void Start(AstarNode[] pathfindingNetwork, DefinitionNode[] definitionNodes, int startNodeIndex, int targetNodeIndex, float neededClearance, PathfindaxCollisionCategory collisionCategory)
{
StartNodeIndex = startNodeIndex;
TargetNodeIndex = targetNodeIndex;
_neededClearance = neededClearance;
_collisionCategory = collisionCategory;
_pathfindingNetwork = pathfindingNetwork;
_definitionNodes = definitionNodes;
_openSet.AssignArray(pathfindingNetwork);
_closedSet.Clear();
_openSet.Add(startNodeIndex);
}
private DijkstraNode[] GenerateNodeNetwork(PathfindaxCollisionCategory collisionCategory)
{
var gridClearanceGenerator = new BrushfireClearanceGenerator(DefinitionNodeGrid, _maxClearance);
var nodeNetwork = new DijkstraNode[DefinitionNodeGrid.NodeCount];
for (var i = 0; i < DefinitionNodeGrid.NodeGrid.Array.Length; i++)
{
ref var definitionNode = ref DefinitionNodeGrid.NodeGrid.Array[i];
nodeNetwork[i] = new DijkstraNode
{
Clearance = _maxClearance == -1 ? int.MaxValue : gridClearanceGenerator.CalculateClearance(ref definitionNode, collisionCategory, _maxClearance)
};
}
/// <summary>
/// Calculates the clearances up to a maximum <paramref name="maxClearance"/>
/// </summary>
/// <param name="definitionNode"></param>
/// <param name="collisionCategory"></param>
/// <param name="maxClearance"></param>
/// <returns></returns>
public float CalculateGridNodeClearances(DefinitionNode definitionNode, PathfindaxCollisionCategory collisionCategory, int maxClearance)
{
var fromCoordinates = new Point2((int)definitionNode.Position.X, (int)definitionNode.Position.Y);
for (var checkClearance = 0; checkClearance < maxClearance; checkClearance++)
{
var nextClearanceIsBlocked = false;
for (var x = 0; x < checkClearance + 1; x++)
{
switch (IsBlocked(x + fromCoordinates.X, checkClearance + fromCoordinates.Y, collisionCategory, fromCoordinates))
{
case BlockType.Current:
return(checkClearance);
case BlockType.Next:
nextClearanceIsBlocked = true;
break;
}
}
for (var y = 0; y < checkClearance; y++)
{
switch (IsBlocked(checkClearance + fromCoordinates.X, y + fromCoordinates.Y, collisionCategory, fromCoordinates))
{
case BlockType.Current:
return(checkClearance);
case BlockType.Next:
nextClearanceIsBlocked = true;
break;
}
}
if (nextClearanceIsBlocked)
{
var isBlocked = true;
for (var i = 0; i < definitionNode.Connections.Count; i++)
{
if ((definitionNode.Connections[i].CollisionCategory & collisionCategory) == 0)
{
isBlocked = false;
}
}
return(isBlocked ? checkClearance : checkClearance + 1);
}
}
return(maxClearance);
}
public AstarNode[] GetCollisionLayerNetwork(PathfindaxCollisionCategory collisionCategory)
{
if (!_nodeNetworks.TryGetValue(collisionCategory, out var pathfindingNetwork))
{
var watch = Stopwatch.StartNew();
pathfindingNetwork = GenerateNodeNetwork(DefinitionNodeNetwork);
foreach (var pathfindNodeGenerator in _pathfindNodeGenerators)
{
pathfindNodeGenerator.Generate(pathfindingNetwork, collisionCategory);
}
Debug.WriteLine($"Generated pathfind nodenetwork in {watch.ElapsedMilliseconds} ms");
_nodeNetworks.Add(collisionCategory, pathfindingNetwork);
}
return(pathfindingNetwork);
}
private DijkstraNode[] GenerateNodeNetwork(PathfindaxCollisionCategory collisionCategory)
{
var gridClearanceGenerator = new GridClearanceGenerator(DefinitionNodeGrid, _maxClearance);
var nodeNetwork = new DijkstraNode[DefinitionNodeGrid.NodeCount];
for (var y = 0; y < DefinitionNodeGrid.NodeGrid.Height; y++)
{
for (var x = 0; x < DefinitionNodeGrid.NodeGrid.Width; x++)
{
var definitionNode = DefinitionNodeGrid.NodeGrid[x, y];
nodeNetwork[definitionNode.Index.Index] = new DijkstraNode(definitionNode)
{
Clearance = _maxClearance == -1 ? int.MaxValue : gridClearanceGenerator.CalculateGridNodeClearances(definitionNode, collisionCategory, _maxClearance)
};
}
}
return(nodeNetwork);
}
public void Generate(AstarNode[] pathfindingNetwork, PathfindaxCollisionCategory collisionCategory)
{
if (_maxClearance < 0)
{
for (var i = 0; i < pathfindingNetwork.Length; i++)
{
pathfindingNetwork[i].Clearance = int.MaxValue;
}
}
else
{
for (int y = 0; y < _definitionNodeGrid.DefinitionNodeArray.Height; y++)
{
for (int x = 0; x < _definitionNodeGrid.DefinitionNodeArray.Width; x++)
{
var definitionNode = _definitionNodeGrid.DefinitionNodeArray[x, y];
var clearance = CalculateGridNodeClearances(definitionNode, collisionCategory, _maxClearance);
pathfindingNetwork[definitionNode.Index.Index].Clearance = clearance;
}
}
}
}
public void SetConnectionCollision(int index, PathfindaxCollisionCategory collisionCategory)
{
Connections[index] = new NodeConnection(Connections[index].To, collisionCategory);
}
public static PathRequest <TPath> Create <TPath>(int start, int end, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.None, byte agentSize = 1)
where TPath : IPath
{
return(new PathRequest <TPath>(start, end, collisionCategory, agentSize));
}
public static PathRequest <TPath> Create <TPath>(IPathfinder <TPath> pathfinder, int start, int end, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.None, byte agentSize = 1)
where TPath : IPath
{
var request = new PathRequest <TPath>(start, end, collisionCategory, agentSize);
pathfinder.RequestPath(request);
return(request);
}
public PathRequest <TPath> RequestPath(DefinitionNode start, DefinitionNode end, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
return(PathRequest.Create(this, start, end, collisionLayer, agentSize));
}
public PathRequest <TPath> RequestPath(float x1, float y1, float x2, float y2, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
return(PathFindAlgorithm.CreatePathRequest(this, DefinitionNodeNetwork, x1, y1, x2, y2, collisionLayer, agentSize));
}
private bool ConnectionIsBlocked(Array2D <DefinitionNode> nodeGrid, int from, int to, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.Cat1)
{
if (from == -1 || to == -1)
{
return(true);
}
var connection = nodeGrid.Array[from].Connections.FirstOrDefault(x => x.To == to);
return((connection.CollisionCategory & collisionCategory) != 0);
}
private void GetCollisionCategory(NodeGridCollisionMask nodeGridCollisionLayers, int x, int y, CollisionDirection tileCollisionShapeSelf, int toX, int toY, CollisionDirection tileCollisionShapeOther, ref PathfindaxCollisionCategory collisionCategory)
{
foreach (var nodeGridCollisionLayer in nodeGridCollisionLayers.Layers)
{
if ((collisionCategory | nodeGridCollisionLayer.CollisionCategory) == collisionCategory)
{
continue;
}
if (DoesCollide(nodeGridCollisionLayer, x, y, tileCollisionShapeSelf) || DoesCollide(nodeGridCollisionLayer, toX, toY, tileCollisionShapeOther))
{
collisionCategory |= nodeGridCollisionLayer.CollisionCategory;
}
}
}
public PathRequest <NodePath> CreatePathRequest(IPathfinder <NodePath> pathfinder, IDefinitionNodeNetwork definitionNodes, float x1, float y1, float x2, float y2, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
DefinitionNode startNode;
DefinitionNode endNode;
switch (definitionNodes)
{
case IDefinitionNodeGrid definitionNodeGrid:
var offset = -GridClearanceHelper.GridNodeOffset(agentSize, definitionNodeGrid.Transformer.Scale);
startNode = definitionNodeGrid.GetNode(x1 + offset.X, y1 + offset.Y);
endNode = definitionNodeGrid.GetNode(x2 + offset.X, y2 + offset.Y);
return(PathRequest.Create(pathfinder, startNode, endNode, collisionLayer, agentSize));
case IDefinitionNodeNetwork definitionNodeNetwork:
startNode = definitionNodeNetwork.GetNode(x1, y1);
endNode = definitionNodeNetwork.GetNode(x2, y2);
return(PathRequest.Create(pathfinder, startNode, endNode, collisionLayer, agentSize));
default:
throw new NotSupportedException($"{definitionNodes.GetType()} is not supported");
}
}
public PathRequest <TPath> RequestPath(int start, int end, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
return(PathRequest.Create(this, 0, 0, PathfindaxCollisionCategory.None, 0));
}
public PathRequest <TPath> RequestPath(float x1, float y1, float x2, float y2, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
return(RequestPath(0, 0, collisionLayer, agentSize));
}
private void DrawNodeConnections(IRenderer renderer, Transformer transformer, Vector2 nodeWorldPosition, NodeConnection[] nodeConnections, DefinitionNode[] definitionNodes, PathfindaxCollisionCategory collisionCategory)
{
foreach (var connection in nodeConnections)
{
if ((connection.CollisionCategory & collisionCategory) != 0)
{
continue;
}
ref var toNode = ref definitionNodes[connection.To];
var vector = (transformer.ToWorld(toNode.Position) - nodeWorldPosition) * 0.5f; //Times 0.5f so we can see the connections in both directions.
renderer.DrawLine(nodeWorldPosition, nodeWorldPosition + vector);
}
public void AddConnection(int to, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.None)
{
Connections = Connections.Append(new NodeConnection(to, collisionCategory));
}
void ICmpInitializable.OnActivate()
{
_rigidBody = GameObj.GetComponent <RigidBody>();
_collisionCategory = (PathfindaxCollisionCategory)_rigidBody.CollisionCategory;
DualityApp.Mouse.ButtonDown += Mouse_ButtonDown;
}
public void AddConnection(DefinitionNode to, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.None)
{
Connections.Add(new NodeConnection(to.Index, collisionCategory));
}
private static List <DefinitionNode> FindPath(AstarNode[] pathfindingNetwork, AstarNode startNode, AstarNode targetNode, float neededClearance, PathfindaxCollisionCategory collisionCategory)
{
try
{
var sw = new Stopwatch();
sw.Start();
var pathSucces = false;
if (startNode == targetNode)
{
return(new List <DefinitionNode> {
targetNode.DefinitionNode
});
}
if (startNode.Clearance >= neededClearance && targetNode.Clearance >= neededClearance)
{
var openSet = new MaxHeap <AstarNode>(pathfindingNetwork.Length);
var closedSet = new HashSet <AstarNode>();
var itterations = 0;
var neighbourUpdates = 0;
openSet.Add(startNode);
while (openSet.Count > 0)
{
itterations++;
var currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
Debug.WriteLine($"NodePath found in {sw.ElapsedMilliseconds} ms. Itterations: {itterations} Neighbourupdates: {neighbourUpdates}");
pathSucces = true;
break;
}
foreach (var connection in currentNode.DefinitionNode.Connections)
{
var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork);
if ((connection.CollisionCategory & collisionCategory) != 0 || closedSet.Contains(toNode))
{
continue;
}
if (toNode.Clearance >= neededClearance)
{
var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier;
if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode))
{
toNode.GCost = newMovementCostToNeighbour;
toNode.HCost = GetDistance(toNode.DefinitionNode, targetNode.DefinitionNode);
toNode.Parent = currentNode.DefinitionNode.Index;
neighbourUpdates++;
if (!openSet.Contains(toNode))
{
openSet.Add(toNode);
}
}
}
}
}
}
if (pathSucces)
{
return(RetracePath(pathfindingNetwork, startNode, targetNode));
}
Debug.WriteLine("Did not find a path :(");
return(null);
}
catch (Exception ex)
{
Debug.WriteLine(ex);
Debugger.Break();
return(null);
}
}
/// <summary>
/// Creates a new <see cref="NodeConnection"/>.
/// </summary>
/// <param name="to">Where this connection is going to</param>
/// <param name="collisionCategory">The <see cref="PathfindaxCollisionCategory"/> of this <see cref="NodeConnection"/></param>
public NodeConnection(NodePointer to, PathfindaxCollisionCategory collisionCategory = PathfindaxCollisionCategory.None)
{
To = to;
CollisionCategory = collisionCategory;
}
public PathRequest <TPath> RequestPath(Vector2 start, Vector2 end, PathfindaxCollisionCategory collisionLayer = PathfindaxCollisionCategory.None, byte agentSize = 1)
{
return(RequestPath(start.X, start.Y, end.X, end.Y, collisionLayer, agentSize));
}
object IPathfindNodeNetwork.GetCollisionLayerNetwork(PathfindaxCollisionCategory collisionCategory) => GetCollisionLayerNetwork(collisionCategory);
/// <summary>
/// Creates a new instance of the <see cref="TilemapCollider"/> and adds the <see cref="RigidBody"/> thats on the same <see cref="GameObject"/>
/// </summary>
/// <param name="tilemapCollider"></param>
public TilemapColliderWithBody(TilemapCollider tilemapCollider)
{
CollisionCategory = (PathfindaxCollisionCategory)tilemapCollider.GameObj.GetComponent <RigidBody>().CollisionCategory;
TilemapCollisionSources = tilemapCollider.CollisionSource.ToArray();
}
