//Some basic operations.
public bool BoundedBy(IVector3 min, IVector3 max)
{
if (X >= min.X && Y >= min.Y && Z >= min.Z &&
X < max.X && Y < max.Y && Z < max.Z)
{
return true;
}
return false;
}
TileType GetTileType( IVector3 xyz )
{
if ( xyz.Z == 0 ) return TileType.Grass;
return TileType.Air;
}
public static double Distance(IVector3 a, IVector3 b)
{
double d = Dot(a, b);
return Math.Sqrt(d);
}
public static int Dot(IVector3 a, IVector3 b)
{
return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
}
private void Reconstruct(Dictionary<IVector3, IVector3> camefrom, IVector3 current, List<IVector3> result)
{
if (camefrom.ContainsKey(current))
{
Reconstruct(camefrom, camefrom[current], result);
}
result.Add(current);
}
private List<IVector3> PassableNodes(IVector3 node)
{
var result = new List<IVector3>();
Tile currentTile = this[node];
TileDeclaration decl = currentTile.Declaration;
if (decl.StairType != StairType.None)
{
if ((decl.StairType & StairType.Up) != 0)
{
IVector3 target = node;
target.Z += 1;
if (this[target].IsPassable)
{
result.Add(target);
}
}
if ((decl.StairType & StairType.Down) != 0)
{
IVector3 target = node;
target.Z -= 1;
if (this[target].IsPassable)
{
result.Add(target);
}
}
}
for (int x = -1; x <= 1; ++x)
{
for (int y = -1; y <= 1; ++y)
{
if (y == 0 && x == 0)
{
continue;
}
IVector3 target = node;
target.X += x;
target.Y += y;
if (target.BoundedBy(IVector3.Zero, Dimensions))
{
Tile tile = this[target];
if (tile.IsPassable)
{
result.Add(target);
}
}
}
}
return result;
}
private double Cost(IVector3 node, IVector3 end)
{
return IVector3.Dot(node, end);
}
// Basic A* implementation.
public List<IVector3> Path(IVector3 start, IVector3 end)
{
Debug.Assert(start.BoundedBy(IVector3.Zero, Dimensions));
Debug.Assert(end.BoundedBy(IVector3.Zero, Dimensions));
List<IVector3> result = new List<IVector3>();
HashSet<IVector3> closed = new HashSet<IVector3>();
HashSet<IVector3> open = new HashSet<IVector3>();
open.Add(start);
var cameFrom = new Dictionary<IVector3, IVector3>();
var goalScore = new Dictionary<IVector3, double>();
goalScore.Add(start, 0);
var guessScore = new Dictionary<IVector3, double>();
guessScore.Add(start, Cost(start, end));
var finalScore = new Dictionary<IVector3, double>();
finalScore.Add(start, guessScore[start]);
while(open.Count != 0)
{
double minscore = finalScore[open.First()];
IVector3 node = open.First();
foreach (var n in open)
{
if (finalScore[n] < minscore)
{
minscore = finalScore[n];
node = n;
}
}
if (node == end)
{
Reconstruct(cameFrom, cameFrom[end], result);
result.Add(end);
return result;
}
open.Remove(node);
closed.Add(node);
foreach (var t in PassableNodes(node))
{
if (closed.Contains(t))
{
continue;
}
bool isBetter = false;
double score = goalScore[node] + IVector3.Distance(node, t);
if (!open.Contains(t))
{
open.Add(t);
isBetter = true;
}
else if (score < goalScore[t])
{
isBetter = true;
}
else
{
isBetter = false;
}
if (isBetter)
{
cameFrom[t] = node;
goalScore[t] = score;
guessScore[t] = Cost(t, end);
finalScore[t] = score + Cost(t, end);
}
}
}
//No path!
return null;
}
public Tile this[IVector3 i]
{
get { return this[i.X, i.Y, i.Z]; }
set { this[i.X, i.Y, i.Z] = value; }
}