here is the code
the error is
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace Tests
{
/// <summary>
/// Author: Roy Triesscheijn (http://www.roy-t.nl)
/// Class providing 3D pathfinding capabilities using A*.
/// Heaviliy optimized for speed therefore uses slightly more memory
/// On rare cases finds the 'almost optimal' path instead of the perfect path
/// this is because we immediately return when we find the exit instead of finishing
/// 'neighbour' loop.
/// </summary>
public static class PathFinder
{
/// <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 SearchNode FindPath(World world, Point3D start, Point3D end)
{
//note we just flip start and end here so you don't have to.
return FindPathReversed(world, end, start);
}
/// <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 SearchNode FindPathReversed(World world, Point3D start, Point3D end)
{
SearchNode startNode = new SearchNode(start, 0, 0, null);
MinHeap openList = new MinHeap();
openList.Add(startNode);
int sx = world.Right;
int sy = world.Top;
int sz = world.Back;
Dictionary<string, PathLoc> boolWorld = new Dictionary<string, PathLoc>();
//create a dictionary to start with
//BitArray brWorld = new BitArray(sx * sy * sz);
//mark start as checked
boolWorld.Add(""+(start.X + sx * (start.Y + start.Z * sy)), new pathLoc(true));
//brWorld.Set(start.X + sx * (start.Y + start.Z * sy), true);
while (openList.HasNext())
{
SearchNode current = openList.ExtractFirst();
if (current.position.GetDistanceSquared(end) <= 3)
{
return new SearchNode(end, current.pathCost + 1, current.cost + 1, current);
}
if (current.position.GetDistance(end) <= 100)
{
for (int i = 0; i < surrounding.Length; i++)
{
Surr surr = surrounding[i];
Point3D tmp = new Point3D(current.position, surr.Point);
//set brWorldIdx to the string
int brWorldIdx = tmp.X + (tmp.Y + tmp.Z * sy) * sx;
//in if statement set 2nd condition to find based of string and be == not found
if (world.PosFree(tmp) && DictionaryBool(""+brWorldIdx, boolWorld) == false && world.IsGrounded(tmp))
{
//create dictionary entry for this
//brWorld.Set(brWorldIdx, true);
boolWorld.Add(""+(brWorldIdx), new pathLoc(true));
int pathCost = current.pathCost + surr.Cost;
int cost = pathCost + tmp.GetDistanceSquared(end);
SearchNode node = new SearchNode(tmp, cost, pathCost, current);
openList.Add(node);
}
}
}
else{
}
}
return null; //no path found
}
//do dictionary shit
private static bool brWorldBool (int id, BitArray brWorld){
return brWorld.Get (id);
}
private static bool DictionaryBool (string Idx, Dictionary<string, pathLoc> boolWorld)
{
pathLoc pathBo = null;
if(boolWorld.TryGetValue(Idx, out pathBo))
{
return pathBo.Bo;
}
else
{
return false;
}
}
class Surr
{
public Surr(int x, int y, int z)
{
Point = new Point3D(x, y, z);
Cost = x * x + y * y + z * z;
}
public Point3D Point;
public int Cost;
}
//Neighbour options 3D, diagonals
private static Surr[] surrounding = new Surr[]{
//Top slice (Y=1)
new Surr(-1,1,1), new Surr(0,1,1), new Surr(1,1,1),
new Surr(-1,1,0), new Surr(0,1,0), new Surr(1,1,0),
new Surr(-1,1,-1), new Surr(0,1,-1), new Surr(1,1,-1),
//Middle slice (Y=0)
new Surr(-1,0,1), new Surr(0,0,1), new Surr(1,0,1),
new Surr(-1,0,0), new Surr(1,0,0), //(0,0,0) is self
new Surr(-1,0,-1), new Surr(0,0,-1), new Surr(1,0,-1),
//Bottom slice (Y=-1)
new Surr(-1,-1,1), new Surr(0,-1,1), new Surr(1,-1,1),
new Surr(-1,-1,0), new Surr(0,-1,0), new Surr(1,-1,0),
new Surr(-1,-1,-1), new Surr(0,-1,-1), new Surr(1,-1,-1)
};
/*
//Neighbour options 3D // 4 dirs
private static Surr[] surrounding = new Surr[]{
//Top slice (Y=1)
//new Surr(0,1,1),
//new Surr(-1,1,0),
new Surr(0,1,0),
//, new Surr(1,1,0),
//new Surr(0,1,-1),
//Middle slice (Y=0)
new Surr(0,0,1),
new Surr(-1,0,0), new Surr(1,0,0), //(0,0,0) is self
new Surr(0,0,-1),
//Bottom slice (Y=-1)
//new Surr(0,-1,1),
//new Surr(-1,-1,0),
new Surr(0,-1,0)
//, new Surr(1,-1,0),
//new Surr(0,-1,-1)
};*/
// 2D
/*
private static Surr[] surrounding = new Surr[]{
new Surr(0,-1,0),
new Surr(1,0,0),
new Surr(0,1,0),
new Surr(-1,0,0)
};*/
}
public class pathLoc
{
public bool Bo;
public pathLoc(bool NBo)
{
Bo = NBo;
}
}
}