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);
}
}
}
public Path FindPath(Vector3 start, Vector3 end)
{
long oldNow = DateTime.Now.Ticks;
bool found = false;
this.end = PositionToVoxel(start); // end refers to start
this.start = PositionToVoxel(end); // start refers to end
open.Clear();
closed.Clear();
Node first = new Node();
first.f = first.g = first.h = 0.0f;
first.pos = this.start;
open[this.start] = first;
//openList.Add(first);
//openPQ.Enqueue(first);
Node current = first;
System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch();
int maxSize = 0;
stopwatch.Start();
bool timeout = false;
while (open.Count > 0)
{
if (stopwatch.ElapsedMilliseconds > 5000)
{
timeout = true;
break;
}
if (open.Count > maxSize)
{
maxSize = open.Count;
}
//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);
//openList.Remove(current);
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);
message = "A * took: " + stopwatch.ElapsedMilliseconds + " milliseconds. Open list: " + maxSize;
}
else
{
if (timeout)
{
message = "A* timed out after 5 seconds. Open list: " + maxSize;
}
else
{
message = "No path found. Open list: " + maxSize;
}
}
if (smooth)
{
SmoothPath(path);
}
return path;
}
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;
//openList.Add(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);
//openList.Remove(current);
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;
Debug.Log("A * took: " + (elapsed / 10000) + " milliseconds");
if (smooth)
{
SmoothPath(path);
}
return path;
}