public override List <IPathable> GetNeighbours(IPathable node)
{
List <IPathable> neighbours = new List <IPathable>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
int xIndex = node.X + x;
int yIndex = node.Y + y;
// Check if neighbour is in borders
if (xIndex >= 0 && xIndex < Width && yIndex >= 0 && yIndex < Height)
{
if (Grid[xIndex, yIndex] == null)
{
continue;
}
if (Mathf.Abs(Grid[xIndex, yIndex].Position.y - node.Position.y) < 1.5f) // TODO: Remove Magic Number!
{
neighbours.Add(Grid[xIndex, yIndex]);
}
}
}
}
return(neighbours);
}
private void OnTriggerExit(Collider other)
{
ISelectable otherSelection = other.GetComponent <ISelectable>();
if (!otherSelection.IsNull() && otherSelection.isInteractable)
{
if (!_hoverTarget.IsNull())
{
if (!_isTriggerDown)
{
_hoverTarget.OnSelectionStateChanged(SelectionState.None);
}
_hoverTarget = null;
}
// We keep track of the position we clicked and when we exit the collider we use the current
// position. This is used to give us two good points.
if (!_selected.IsNull())
{
if (!otherSelection.IsNull() && otherSelection == _selected)
{
_isAwaitingEscape = false;
IPathable pathable = _selected as IPathable;
if (!pathable.IsNull())
{
pathable.AddPathPosition(_triggerPressedPosition);
pathable.AddPathPosition(_tip.position);
}
}
}
}
}
public Node(int gScore, int hScore, IPathable nodeObject, Node parentNode = null)
{
G = gScore;
H = hScore;
NodeObject = nodeObject;
ParentNode = parentNode;
}
public void AddConnection(IPathable node, float cost)
{
if (node.Equals(this))
{
return;
}
costByNode[node] = cost;
}
public void UnregisterPathable(IPathable <Entity> pathable)
{
if (pathable == null)
{
return;
}
_queuedRemovePathables.Enqueue(pathable);
}
public void RegisterPathable(IPathable <Entity> pathable)
{
if (pathable == null)
{
return;
}
this.Pathables.Add(pathable);
}
public int CompareTo(IPathable other)
{
int compare = F.CompareTo(other.F);
if (compare == 0)
{
compare = H.CompareTo(other.H);
}
return(-compare);
}
public PathFinder(IPathable grid, Vector2 start, List <Vector2> targets)
{
_grid = grid;
_start = start;
_targets = targets;
// Dictionary would be more efficient
traversed_squares = new List <Vector2>();
end_nodes = new List <Node>();
}
public void AddTile(IHex t, IPathable navigator)
{
if (this._tiles.Count == 0)
{
this._score += navigator.GetTileTraversalCost(navigator.GetCurrentTile(), t);
}
else
{
this._score += navigator.GetTileTraversalCost(this._tiles[this._tiles.Count - 1], t);
}
this._tiles.Add(t);
}
private void ProcessPathableState(IPathable <Entity> pathable)
{
if (pathable.MapId == GameService.Gw2Mumble.CurrentMap.Id || pathable.MapId == -1)
{
pathable.Active = true;
GameService.Pathing.RegisterPathable(pathable);
}
else if (GameService.Graphics.World.Entities.Contains(pathable.ManagedEntity))
{
pathable.Active = false;
GameService.Pathing.UnregisterPathable(pathable);
}
}
private Path GetPathViaSourceAdjacentToGoal(IHex s, IHex g, IPathable navigator)
{
foreach (var neighbor in s.GetAdjacent())
{
if (neighbor.Equals(g))
{
var trimmed = new Path();
trimmed.AddTile(g, navigator);
return(trimmed);
}
}
return(null);
}
private void ProcessPathableState(IPathable <Entity> pathable)
{
if (pathable.MapId == Player.MapId || pathable.MapId == -1)
{
//pathable.Active = true;
Graphics.World.Entities.Add(pathable.ManagedEntity);
}
else if (Graphics.World.Entities.Contains(pathable.ManagedEntity))
{
//pathable.Active = false;
Graphics.World.Entities.Remove(pathable.ManagedEntity);
}
}
public Path GetPath(IHex s, IHex g, IPathable navigator)
{
var quickPath = this.GetPathViaSourceAdjacentToGoal(s, g, navigator);
if (quickPath != null)
{
return(quickPath);
}
var goalData = this.GetPathHelperData(g);
var sourceData = this.GetPathHelperData(s);
var validPaths = new List <Path>();
while (sourceData.OpenSet.Count > 0 && goalData.OpenSet.Count > 0)
{
this.GetPathHelperIterateData(sourceData, navigator);
this.GetPathHelperIterateData(goalData, navigator);
if (sourceData.PathsToGoal.Count > 0 && goalData.PathsToGoal.Count > 0)
{
foreach (var sourcePath in sourceData.PathsToGoal)
{
foreach (var goalPath in goalData.PathsToGoal)
{
if (sourcePath.GetLastTile().Equals(goalPath.GetLastTile()))
{
var valid = new Path();
foreach (var tile in sourcePath.GetTiles())
{
valid.AddTile(tile, navigator);
}
goalPath.GetTiles().Reverse();
for (int i = 1; i < goalPath.GetTiles().Count; i++)
{
valid.AddTile(goalPath.GetTiles()[i], navigator);
}
valid.AddTile(g, navigator);
validPaths.Add(valid);
}
}
}
}
if (validPaths.Count > 0)
{
var bestPath = validPaths.OrderBy(x => x.Score).ToList()[0];
return(this.TryOptimizePath(bestPath, s, g, navigator));
}
}
return(null);
}
Vector3[] RetracePath(IPathable startNode, IPathable endNode)
{
List <IPathable> path = new List <IPathable>();
IPathable currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
path.Add(startNode);
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return(waypoints);
}
private Path TryOptimizePath(Path p, IHex s, IHex g, IPathable navigator)
{
var extendedSet = ListUtil <IHex> .ShallowClone(p.GetTiles());
foreach (var tile in p.GetTiles())
{
foreach (var neighbor in tile.GetAdjacent())
{
if (!extendedSet.Contains(neighbor))
{
extendedSet.Add(neighbor);
}
}
}
return(this.GetBruteForcePathViaFiniteSet(extendedSet, s, g, navigator));
}
/// <summary>
/// Returns distance from first point to second
/// </summary>
/// <param name="nodeA"></param>
/// <param name="nodeB"></param>
/// <returns></returns>
int GetDistance(IPathable nodeA, IPathable nodeB)
{
int distX = Mathf.Abs(nodeA.X - nodeB.X);
int distY = Mathf.Abs(nodeA.Y - nodeB.Y);
//TODO: This works for grids! Make it more general
// Horizontal and vertical movement cost 10
// Diagonal movement cost 14
if (distX > distY)
{
return((int)((14 * distY + 10 * (distX - distY)) * CostMultiplier));
}
return((int)((14 * distX + 10 * (distY - distX)) * CostMultiplier));
}
private void UpdateFlightPath(Vector3 previous, Vector3 current)
{
Vector3 delta = previous - current;
float magnitude = Vector3.Magnitude(delta) * _playerTransformRuntimeSet.scale;
_nextWaypoint -= magnitude;
if (_nextWaypoint <= 0)
{
_nextWaypoint = _runtimeSet.waypointDelta;
IPathable pathable = _selected as IPathable;
if (!pathable.IsNull())
{
pathable.AddPathPosition(current);
}
}
}
private Path TrimRedundantPathing(Path path, IHex g, IPathable navigator)
{
var validPaths = new List <Path>();
var openSet = path.GetTiles();
var initPath = new Path();
var pathDict = new Dictionary <Pair <int, int>, Path>();
var firstTile = path.GetTiles()[0];
initPath.AddTile(firstTile, navigator);
pathDict.Add(new Pair <int, int>(firstTile.GetCol(), firstTile.GetRow()), initPath);
while (openSet.Count > 0)
{
var tile = openSet.ElementAt(0);
foreach (var neighbor in tile.GetAdjacent())
{
if (openSet.Contains(neighbor))
{
var pathKey = new Pair <int, int>(tile.GetCol(), tile.GetRow());
var previousPath = pathDict[pathKey];
var newPath = previousPath.DeepCopy();
newPath.AddTile(neighbor, navigator);
var newKey = new Pair <int, int>(neighbor.GetCol(), neighbor.GetRow());
if (!pathDict.ContainsKey(newKey))
{
pathDict.Add(newKey, newPath);
}
else
{
if (newPath.Score < pathDict[newKey].Score)
{
pathDict[newKey] = newPath;
}
}
if (neighbor == g)
{
validPaths.Add(newPath);
}
}
}
openSet.Remove(tile);
}
var bestPath = validPaths.OrderBy(x => x.Score).ToList()[0];
return(bestPath);
}
/// <summary>
/// This method should always be called for new patterns.
/// </summary>
/// <typeparam name="TANode"></typeparam>
/// <typeparam name="TRel"></typeparam>
/// <typeparam name="TBNode"></typeparam>
/// <param name="source"></param>
/// <param name="A"></param>
/// <param name="R"></param>
/// <param name="B"></param>
/// <param name="dir"></param>
/// <param name="testARBForNull"></param>
/// <returns></returns>
internal static IPatternedPath SharedPattern <TANode, TRel, TBNode>
(this IPathable source, string A, string R, string B, RelationshipDirection?dir,
bool testARBForNull = true)
{
if (testARBForNull && string.IsNullOrWhiteSpace(A) && string.IsNullOrWhiteSpace(R) &&
string.IsNullOrWhiteSpace(B))
{
//you must provide at least one variable
throw new InvalidOperationException(Messages.NullARBVariablesError);
}
var builder = source as IPathBuilder;
var oldPath = source as Path;
Path path = null;
if (builder != null)
{
path = new Path <TANode, TRel, TBNode>(builder);
}
else if (oldPath != null)
{
//create new path as we may have new entity types.
path = new Path <TANode, TRel, TBNode>(oldPath);
}
else
{
throw new ArgumentException(Messages.PathableNotRecognizedError, nameof(source));
}
var pattern = path.Pattern as Pattern;
pattern.AVariable = A;
pattern.AType = typeof(TANode);
pattern.RVariable = R;
pattern.RType = typeof(TRel);
pattern.BVariable = B;
pattern.BType = typeof(TBNode);
pattern.Direction = dir;
return(path);
}
private void onTriggerReleased()
{
if (!_selected.IsNull())
{
_isAwaitingEscape = false;
_selected.OnSelectionStateChanged(SelectionState.None);
_selected = null;
IPathable pathable = _selected as IPathable;
if (!pathable.IsNull())
{
pathable.EndPath();
}
if (_hoverTarget != null)
{
_hoverTarget.OnSelectionStateChanged(SelectionState.Highlighted);
}
}
}
private IEnumerator Start()
{
for (int i = 0; i < _spawnCount; i++)
{
GameObject plane = planeFactory.CreateRandomPlane(Vector3.zero, Quaternion.identity);
IPathable pathable = plane.GetComponent <IPathable>();
if (!pathable.IsNull() && nodes.Count > 0)
{
pathable.StartPath(nodes[0].transform.position);
for (int x = 1; x < nodes.Count; x++)
{
pathable.AddPathPosition(nodes[x].transform.position);
}
pathable.EndPath();
}
yield return(new WaitForSeconds(1f));
}
}
internal static IPatternedPath SharedPattern <TANode, TRel, TBNode>
(this IPathable source, LambdaExpression relationship, string A, string R, string B, RelationshipDirection?dir,
bool testARBForNull = false)
{
//for the sake of our tests, leave this here, and don't fall for the VS intellisense refactoring bait.
if (relationship == null)
{
throw new ArgumentNullException(nameof(relationship));
}
var path = SharedPattern <TANode, TRel, TBNode>(source, A, R, B, dir,
testARBForNull); //set the other properties
var pattern = path.Pattern as Pattern;
pattern.ABSelector = relationship;
pattern.Direction = dir;
return(path);
}
private void OnTriggerPressed()
{
_triggerPressedPosition = _tip.position;
if (!_hoverTarget.IsNull())
{
_isAwaitingEscape = true;
_selected = _hoverTarget;
IPathable pathable = _selected as IPathable;
_hasPathableSelected = !pathable.IsNull();
// Only pathable objects can have paths
if (_hasPathableSelected)
{
pathable.StartPath(_tip.position);
}
_selected.OnSelectionStateChanged(SelectionState.Pressed);
}
}
private void GetPathHelperIterateData(GetPathDataClass data, IPathable navigator)
{
var tile = data.OpenSet.ElementAt(0);
foreach (var neighbor in tile.GetAdjacent())
{
var neighborKey = new Pair <int, int>(neighbor.GetCol(), neighbor.GetRow());
if (neighbor.GetCurrentOccupant() == null)
{
if (!data.ClosedSet.Contains(neighborKey))
{
data.OpenSet.Add(neighbor);
}
var pathKey = new Pair <int, int>(tile.GetCol(), tile.GetRow());
var previousPath = data.PathsToGoalDict[pathKey];
var newPath = previousPath.DeepCopy();
newPath.AddTile(neighbor, navigator);
var newKey = new Pair <int, int>(neighbor.GetCol(), neighbor.GetRow());
if (!data.PathsToGoalDict.ContainsKey(newKey))
{
data.PathsToGoalDict.Add(newKey, newPath);
}
else
{
if (newPath.Score < data.PathsToGoalDict[newKey].Score)
{
data.PathsToGoalDict[newKey] = newPath;
}
}
data.PathsToGoal.Add(newPath);
}
else
{
data.ClosedSet.Add(neighborKey);
}
}
data.ClosedSet.Add(new Pair <int, int>(tile.GetCol(), tile.GetRow()));
data.OpenSet.Remove(tile);
}
public NodePath Find(IPathable startNode, IPathable endNode)
{
NodePath path = null;
List <Node> openList = new List <Node>();
List <Node> closedList = new List <Node>();
Node currentNode = null;
int currentGScore = 0;
openList.Add(new Node(currentGScore, 0, startNode));
while (openList.Count > 0)
{
var lowest = openList.Min(n => n.F);
currentNode = openList.First(n => n.F == lowest);
closedList.Add(currentNode);
openList.Remove(currentNode);
if (closedList.FirstOrDefault(n => n.NodeObject == endNode) != null)
{
break; // Target is in closed list; Path has been found.
}
var neighbors = currentNode.NodeObject.GetNeighbors();
currentGScore++;
foreach (var neighbor in neighbors)
{
if (closedList.FirstOrDefault(n => n.NodeObject == neighbor) != null)
{
continue; // Neighbor is in closed list, so ignore.
}
if (!neighbor.IsPathable)
{
continue;
}
var newNode = new Node(
currentGScore,
neighbor.GetDistanceEstimate(endNode),
neighbor,
currentNode
);
if (openList.FirstOrDefault(n => n.NodeObject == newNode.NodeObject) == null)
{
// Neighbor is not in open list, so compute scores and add.
openList.Insert(0, newNode);
}
else
{
if (currentGScore + newNode.G < newNode.F)
{
// If current path is better than previous,
// update to new path
newNode.G = currentGScore;
newNode.ParentNode = currentNode;
}
}
}
}
if (currentNode.GetPath().ContainsNode(endNode))
{
path = currentNode.GetPath();
}
return(path);
}
private void ProcessRemovedPathable(IPathable <Entity> pathable)
{
Graphics.World.Entities.Remove(pathable.ManagedEntity);
this.Pathables.Remove(pathable);
}
public float Cost(IPathable neighbor)
{
return(costByNeighbor.TryGetValue(neighbor, out var v) ? v : float.MaxValue);
}
public Path GetBruteForcePathViaFiniteSet(List <IHex> set, IHex s, IHex g, IPathable navigator)
{
var validPaths = new List <Path>();
var openSet = new List <IHex>()
{
s
};
foreach (var tile in set)
{
if (!tile.Equals(s))
{
openSet.Add(tile);
}
}
bool found = false;
var pathDict = new Dictionary <Pair <int, int>, Path>();
var closedSet = new List <Pair <int, int> >();
var initPath = new Path();
initPath.AddTile(s, navigator);
pathDict.Add(new Pair <int, int>(s.GetCol(), s.GetRow()), initPath);
while (openSet.Count > 0 && !found)
{
var tile = openSet.ElementAt(0);
foreach (var neighbor in tile.GetAdjacent())
{
var neighborKey = new Pair <int, int>(neighbor.GetCol(), neighbor.GetRow());
if (neighbor.GetCurrentOccupant() == null)
{
var innerKey = new Pair <int, int>(tile.GetCol(), tile.GetRow());
var previousPath = pathDict[innerKey];
var newPath = previousPath.DeepCopy();
newPath.AddTile(neighbor, navigator);
var newKey = new Pair <int, int>(neighbor.GetCol(), neighbor.GetRow());
if (!pathDict.ContainsKey(newKey))
{
pathDict.Add(newKey, newPath);
}
else
{
if (newPath.Score < pathDict[newKey].Score)
{
pathDict[newKey] = newPath;
}
}
if (neighbor == g)
{
validPaths.Add(newPath);
found = true;
}
}
else
{
closedSet.Add(neighborKey);
}
}
closedSet.Add(new Pair <int, int>(tile.GetCol(), tile.GetRow()));
openSet.Remove(tile);
}
if (validPaths.Count > 0)
{
var bestPath = validPaths.OrderBy(x => x.Score).ToList()[0];
return(bestPath);
}
else
{
return(initPath);
}
}
public abstract List <IPathable> GetNeighbours(IPathable node);
/// <summary>
/// Search for a path.
/// </summary>
/// <param name="startPosition"></param>
/// <param name="targetPosition"></param>
/// <returns></returns>
public IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition)
{
_stopWatch = new Stopwatch();
_stopWatch.Start();
Vector3[] waypoints = new Vector3[0];
IPathable startNode = Graph.GetNode(startPosition);
IPathable targetNode = Graph.GetNode(targetPosition);
bool isPathFound = false;
if (startNode.IsWalkable && targetNode.IsWalkable)
{
Heap <IPathable> openSet = new Heap <IPathable>(Graph.NodeCount());
HashSet <IPathable> closedSet = new HashSet <IPathable>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
IPathable currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
isPathFound = true;
break;
}
foreach (IPathable neighbour in Graph.GetNeighbours(currentNode))
{
if (!neighbour.IsWalkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.G + GetDistance(currentNode, neighbour) + neighbour.Penalty;
if (newMovementCostToNeighbour < neighbour.G || !openSet.Contains(neighbour))
{
neighbour.G = newMovementCostToNeighbour;
neighbour.H = GetDistance(neighbour, targetNode);
neighbour.Parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (isPathFound)
{
waypoints = RetracePath(startNode, targetNode);
}
else
{
UnityEngine.Debug.Log("Search path failed!");
}
_stopWatch.Stop();
TimeSpan ts = _stopWatch.Elapsed;
//UnityEngine.Debug.Log(">>> PathFinding" + " completed in " + ts.TotalMilliseconds + "ms!");
yield return(null);
_pathManager.FinishedProcessingPath(waypoints, isPathFound);
}
