public static List <Vector3> GetPath(Vector3 start, Vector3 exit, bool[] obstaclesArray, MapGrid grid)
{
VertexPosition startVertex = new VertexPosition(start);
VertexPosition exitVertex = new VertexPosition(exit);
List <Vector3> path = new List <Vector3>();
List <VertexPosition> openedList = new List <VertexPosition>();
HashSet <VertexPosition> closedList = new HashSet <VertexPosition>();
startVertex.estimatedCost = ManhattanDistance(startVertex, exitVertex);
openedList.Add(startVertex);
VertexPosition currentVertex = null;
while (openedList.Count > 0)
{
openedList.Sort();
currentVertex = openedList[0];
if (currentVertex.Equals(exitVertex))
{
while (currentVertex != startVertex)
{
path.Add(currentVertex.Position);
currentVertex = currentVertex.previousVertex;
}
path.Reverse();
break;
}
var arrayOfNeighbors = FindNeighborsFor(currentVertex, grid, obstaclesArray);
foreach (var neighbor in arrayOfNeighbors)
{
if (neighbor == null || closedList.Contains(neighbor))
{
continue;
}
if (neighbor.IsTaken == false)
{
var totalCost = currentVertex.totalCost + 1;
var neighborEstimatedCost = ManhattanDistance(neighbor, exitVertex);
neighbor.totalCost = totalCost;
neighbor.previousVertex = currentVertex;
neighbor.estimatedCost = totalCost + neighborEstimatedCost;
if (openedList.Contains(neighbor) == false)
{
openedList.Add(neighbor);
}
}
}
closedList.Add(currentVertex);
openedList.Remove(currentVertex);
}
return(path);
}
public void TestVertexPosition()
{
Assert.That(VertexPosition.VertexDeclaration.VertexStride, Is.EqualTo(12));
var vertexElements = VertexPosition.VertexDeclaration.GetVertexElements();
Assert.That(vertexElements, Has.Length.EqualTo(1));
Assert.That(vertexElements[0].Offset, Is.EqualTo(0));
Assert.That(vertexElements[0].UsageIndex, Is.EqualTo(0));
Assert.That(vertexElements[0].VertexElementFormat, Is.EqualTo(VertexElementFormat.Vector3));
Assert.That(vertexElements[0].VertexElementUsage, Is.EqualTo(VertexElementUsage.Position));
var vertex1 = new VertexPosition(Vector3.One);
var vertex2 = new VertexPosition(Vector3.One);
var vertex3 = new VertexPosition(Vector3.Zero);
Assert.That(vertex1 == vertex2, Is.True);
Assert.That(vertex1 != vertex2, Is.False);
Assert.That(vertex1 == vertex3, Is.False);
Assert.That(vertex1 != vertex3, Is.True);
Assert.That(vertex1.Equals(vertex2), Is.True);
Assert.That(vertex1.Equals(vertex3), Is.False);
}