So first off. The problem as previously pointed out is because the path finds the ‘first’ of the shortest paths. There can be multiple paths with the same cost, we just need to find the ‘first’ one.
You don’t like what’s the first path, since it’s arbitrary.
So to resolve your problem, you need figure out what parameters cause the first, and adjust.
And that has to do with 2 things, your sort algorithm, and the order in which neighbours are found.
Noting that your ‘neighbours’ enumeration is going to grab them in the same order every time… so if say it started at 12 o’clock and returned all neighbours counter-clockwise. When heading left the first shortest path found is the upper left corner, but when heading right it’s the lower right corner.
So lets get into your algorithm… first off this sort algorithm:
for (int i = 1; i < openSet.Count; i++)// Skips this on the first run, we already know our current tile
{
if (openSet[i].fCost < currentTile.fCost || openSet[i].fCost == currentTile.fCost)
{
if (openSet[i].hCost < currentTile.hCost)
{
currentTile = openSet[i];
}
}
}
this isn’t necessarily a bug, this here is just… weird:
openSet[i].fCost < currentTile.fCost || openSet[i].fCost == currentTile.fCost
There is a less than or equal operator that does both steps in one:
openSet[i].fCost <= currentTile.fCost
Next up is this:
if (openSet[i].hCost < currentTile.hCost)
Now I don’t know how you implement your ‘fCost’ property… it doesn’t appear to be a field since you don’t ever set it. So is it a property getter that sums g and h together?
However it is, f should be g + h. Which means the previous f cost comparison should already take care of this. No need to compare the h cost again.
Next I want to get on your heuristic:
public int GetDistance(Tile A, Tile B)
{
int distance = (int)(Mathf.Abs(A.CubeCoord.x - B.CubeCoord.x) + Mathf.Abs(A.CubeCoord.y - B.CubeCoord.y) + Mathf.Abs(A.CubeCoord.z - B.CubeCoord.z)) / 2;
return distance;
}
So this appears to be a halved manhattan distance? Not sure what a CubeCoord is, so not sure what implications the z element makes. If that relates to height, or somethign completely different.
The thing I’m wondering though is why is it an int?
Especially since you halve it…
This loss of fractional granularity can make 2 near nodes appear identical distances rather than one being closer than the other. Not sure the scale of your nodes, but since they’re hexagonal, I’m fairly certain you get a lot of fractional bits which you’re just tossing away.
Might want to change that to a float.
Also… is the g and f cost int as well? Probably shouldn’t be either.
…
Up to this point this should resolve a fair amount of your issues. It should definitely help in removing so much arc from the right picture you posted.
If it’s not quite what you want though, you have more options.
You can change up your heuristic. Currently the halved manhattan heuristic you use already will get a huge bump if changed to floats. But it’s still not perfect. You could change to a squared distance heuristic (essentially the distance formula without performing the costly squareroot), or the distance formula direct. Both of which will result in far more accurate heuristics.
You can apply post smoothing to your path. Basically you can average out your nodes using various algorithms to create a less jaggy path… downside being that it reduces accuracy from a node to node basis.
…
now for some optimization tips
Your sort algorithm is slow.
Personally I use a BinaryHeap:
The comparer I add to it (that sorts the heap) compares the f cost (you could make it compare even more).
As you can see here in my Astar implementation (the stepped portion of the code removed, but exists on the github):
using System;
using System.Collections.Generic;
using System.Linq;
using com.spacepuppy.Collections;
namespace com.spacepuppy.Graphs
{
public class AStarPathResolver<T> : ISteppingPathResolver<T> where T : class
{
#region Fields
private IGraph<T> _graph;
private IHeuristic<T> _heuristic;
private BinaryHeap<VertexInfo> _open;
private HashSet<T> _closed = new HashSet<T>();
private HashSet<VertexInfo> _tracked = new HashSet<VertexInfo>();
private List<T> _neighbours = new List<T>();
private T _start;
private T _goal;
private bool _calculating;
#endregion
#region CONSTRUCTOR
public AStarPathResolver(IGraph<T> graph, IHeuristic<T> heuristic)
{
_graph = graph;
_heuristic = heuristic;
_open = new BinaryHeap<VertexInfo>(graph.Count, VertexComparer.Default);
}
#endregion
#region Properties
public bool IsWorking
{
get { return _calculating; }
}
#endregion
#region IPathResolver Interface
public T Start
{
get { return _goal; }
set
{
if (_calculating) throw new InvalidOperationException("Cannot update start node when calculating.");
_goal = value;
}
}
public T Goal
{
get { return _start; }
set
{
if (_calculating) throw new InvalidOperationException("Cannot update goal node when calculating.");
_start = value;
}
}
public IList<T> Reduce()
{
if (_calculating) throw new InvalidOperationException("PathResolver is already running.");
if (_graph == null || _heuristic == null || _start == null || _goal == null) throw new InvalidOperationException("PathResolver is not initialized.");
var lst = new List<T>();
this.Reduce(lst);
return lst;
}
public int Reduce(IList<T> path)
{
if (_calculating) throw new InvalidOperationException("PathResolver is already running.");
if (_graph == null || _heuristic == null || _start == null || _goal == null) throw new InvalidOperationException("PathResolver is not initialized.");
this.Reset();
_calculating = true;
try
{
_open.Add(this.CreateInfo(_start, _heuristic.Weight(_start), _goal));
while (_open.Count > 0)
{
var u = _open.Pop();
if (u.Node == _goal)
{
int cnt = 0;
while (u.Next != null)
{
path.Add(u.Node);
u = u.Next;
cnt++;
}
path.Add(u.Node);
return cnt + 1;
}
_closed.Add(u.Node);
_graph.GetNeighbours(u.Node, _neighbours);
var e = _neighbours.GetEnumerator();
while(e.MoveNext())
{
var n = e.Current;
if (_closed.Contains(n)) continue;
float g = u.g + _heuristic.Distance(u.Node, n) + _heuristic.Weight(n);
int i = GetInfo(_open, n);
if (i < 0)
{
var v = this.CreateInfo(n, g, _goal);
v.Next = u;
_open.Add(v);
}
else if (g < _open[i].g)
{
var v = _open[i];
v.Next = u;
v.g = g;
v.f = g + v.h;
_open.Update(i);
}
}
_neighbours.Clear();
}
}
finally
{
this.Reset();
}
return 0;
}
private VertexInfo CreateInfo(T node, float g, T goal)
{
var v = _pool.GetInstance();
v.Node = node;
v.Next = null;
v.g = g;
v.h = _heuristic.Distance(node, goal);
v.f = g + v.h;
_tracked.Add(v);
return v;
}
private static int GetInfo(BinaryHeap<VertexInfo> heap, T node)
{
for (int i = 0; i < heap.Count; i++)
{
if (heap[i].Node == node) return i;
}
return -1;
}
public void Reset()
{
_steppedCompletedParentNode = null;
if(_tracked.Count > 0)
{
var e = _tracked.GetEnumerator();
while (e.MoveNext())
{
_pool.Release(e.Current);
}
}
_open.Clear();
_closed.Clear();
_tracked.Clear();
_calculating = false;
}
#endregion
#region Special Types
private static ObjectCachePool<VertexInfo> _pool = new ObjectCachePool<VertexInfo>(-1, () => new VertexInfo(), (v) =>
{
v.Node = null;
v.Next = null;
v.g = 0f;
v.h = 0f;
v.f = 0f;
});
private class VertexInfo
{
public T Node;
public VertexInfo Next;
public float g;
public float h;
public float f;
}
private class VertexComparer : IComparer<VertexInfo>
{
public readonly static VertexComparer Default = new VertexComparer();
public int Compare(VertexInfo x, VertexInfo y)
{
return y.f.CompareTo(x.f);
}
}
#endregion
}
}
Note the comparer used:
private class VertexComparer : IComparer<VertexInfo>
{
public readonly static VertexComparer Default = new VertexComparer();
public int Compare(VertexInfo x, VertexInfo y)
{
return y.f.CompareTo(x.f);
}
}
The HashSet you used for the closedSet is spot on. Nice fast ‘contains’ checks there.
I also shoot for allowing the calling code to pass in its own collection to add to… it allows for easy recycling.
Also this portion of your code here:
if (!kindofTile.isMovable || closedSet.Contains(neighbour) || neighbour.isOccupied)
{
continue;
}
So you may notice that in my code I do something like this when calculating the g cost:
float g = u.g + _heuristic.Distance(u.Node, n) + _heuristic.Weight(n);
Weight is a nice addition you can add to a node.
You can put a weight on a node to say it’s less desirable to walk on. Lets say you have a road, then thick grassy area to the side, and a swamp beyond that. The road might have a Weight of 0, the grass a 10, and the swamp a 50. This would allow the pathfinder to prefer the road even if the way through the swamp is technically shorter. But because the swamp is a pain in the butt to walk through (maybe you slow their speed, or there is attack rates higher in there), they would rather walk around the swamp… unless it’s the only path allowed.
Well you might be asking what about ‘movable/occupied’… such as tiles that can’t be walked whatsoever.
Use very large numbers for weight, upto and including PositiveInfinity.
Your sort algorithm is always going to favor a path that doesn’t take very large weight.
BUT, what if you want to know for 100% that a path exists… it just happens that someone is in your way. Allowing for weights, it’ll last ditch use the path through the high weighted node, but when it returns as completed, you know that your path could get there, but is just blocked by something, because the cost to get their is so high.
