public Path FindPath(Vector3 start, Vector3 end)
{
long oldNow = DateTime.Now.Ticks;
bool found = false;
this.end.X = (float)Math.Round(start.X); this.end.Y = (float)Math.Round(start.Y); this.end.Z = (float)Math.Round(start.Z);
this.start.X = (float)Math.Round(end.X); this.start.Y = (float)Math.Round(end.Y); this.start.Z = (float)Math.Round(end.Z);
open.Clear();
closed.Clear();
Node first = new Node();
first.f = first.g = first.h = 0.0f;
first.pos = this.start;
open[this.start] = first;
openPQ.Enqueue(first);
Node current = first;
while (open.Count > 0)
{
/*
current = openPQ.Dequeue();
float min = current.f;
*/
// Get the top of the q
float min = float.MaxValue;
foreach (Node node in open.Values)
{
if (node.f < min)
{
current = node;
min = node.f;
}
}
if (current.pos.Equals(this.end))
{
found = true;
break;
}
addAdjacentNodes(current);
open.Remove(current.pos);
closed[current.pos] = current;
}
Path path = new Path();
if (found)
{
while (!current.pos.Equals(this.start))
{
path.Waypoints.Add(current.pos);
current = current.parent;
}
path.Waypoints.Add(current.pos);
}
long elapsed = DateTime.Now.Ticks - oldNow;
System.Console.WriteLine("A * took: " + (elapsed / 10000) + " milliseconds");
if (smooth)
{
SmoothPath(path);
}
return path;
}
public void SmoothPath(Path path)
{
List<Vector3> wayPoints = path.Waypoints;
if (wayPoints.Count < 3)
{
return;
}
int current;
int middle;
int last;
current = 0;
middle = current + 1;
last = current + 2;
while (last != wayPoints.Count)
{
Vector3 point0, point2;
point0 = wayPoints[current];
point2 = wayPoints[last];
point0.Y = 0;
point2.Y = 0;
if ((RayTrace(point0, point2)))
{
current++;
middle++;
last++;
}
else
{
wayPoints.RemoveAt(middle);
}
}
}