public int GetDistanceSquared(Point3D point)
{
int dx = this.X - point.X;
int dy = this.Y - point.Y;
int dz = this.Z - point.Z;
return (dx * dx) + (dy * dy) + (dz * dz);
}
/// <summary>
/// Method that switfly finds the best path from start to end. Doesn't reverse outcome
/// </summary>
/// <returns>The end breadcrump where each next is a step back)</returns>
private static BreadCrumb FindPathReversed(Level world, BotMap level, Point3D start, Point3D end)
{
MinHeap<BreadCrumb> openList = new MinHeap<BreadCrumb>(256);
BreadCrumb[, ,] brWorld = new BreadCrumb[world.Size.x, world.Size.y, world.Size.z];
BreadCrumb node;
Point3D tmp;
int cost;
int diff;
BreadCrumb current = new BreadCrumb(start);
current.cost = 0;
BreadCrumb finish = new BreadCrumb(end);
try
{
brWorld[current.position.X, current.position.Y, current.position.Z] = current;
}
catch { return current; }
openList.Add(current);
while (openList.Count > 0)
{
//Find best item and switch it to the 'closedList'
current = openList.ExtractFirst();
current.onClosedList = true;
//Find neighbours
for (int i = 0; i < surrounding.Length; i++)
{
tmp = current.position + surrounding[i];
if ((tmp.X <= -1 || tmp.Y <= -1 || tmp.Z <= -1) || (tmp.X >= level.Size.x || tmp.Y >= level.Size.y || tmp.Z >= level.Size.z))
break;
TriBool block = false;
try
{
block = level.AirMap[tmp.X, tmp.Z, tmp.Y]; //Check if block is air
}
catch { }
if (block != TriBool.Unknown)
{
//Check if we've already examined a neighbour, if not create a new node for it.
if (brWorld[tmp.X, tmp.Y, tmp.Z] == null)
{
node = new BreadCrumb(tmp);
brWorld[tmp.X, tmp.Y, tmp.Z] = node;
}
else
{
node = brWorld[tmp.X, tmp.Y, tmp.Z];
}
//If the node is not on the 'closedList' check it's new score, keep the best
if (!node.onClosedList)
{
diff = 0;
if (current.position.X != node.position.X)
{
diff += 1;
}
if (current.position.Y != node.position.Y)
{
diff += 1;
}
if (current.position.Z != node.position.Z)
{
diff += 1;
}
if (block == false) //Solid but breakable, allows bot to go through solid areas
{
diff += 50;
}
cost = current.cost + diff + node.position.GetDistanceSquared(end);
if (cost < node.cost)
{
node.cost = cost;
node.next = current;
}
//If the node wasn't on the openList yet, add it
if (!node.onOpenList)
{
//Check to see if we're done
if (node.Equals(finish))
{
node.next = current;
return node;
}
node.onOpenList = true;
openList.Add(node);
}
}
}
}
}
return null; //no path found
}
/// <summary>
/// Method that switfly finds the best path from start to end.
/// </summary>
/// <returns>The starting breadcrumb traversable via .next to the end or null if there is no path</returns>
public static BreadCrumb FindPath(Level world, BotMap bm, Point3D start, Point3D end)
{
//note we just flip start and end here so you don't have to.
return FindPathReversed(world, bm, end, start);
}
