public List <Vector3> FindFlatRoute(Vector3 origin, Vector3 destination)
{
var open = new MinHeap <RouteElem>(); //Хипа вместо приоритетой очереди
var orig = FindVertex(origin); //Находим вершину старт
var dest = FindVertex(destination); //Вершину, ближайшую к точку назначения
var closed = new List <Vector3>(); //Список пройденных вершин
if (orig == null || dest == null)
{
throw new ArithmeticException();
}
var first = new RouteElem(orig, 0, destination, null);
open.Add(first); //Добавляем стартовый элемент в хипу
RouteElem current = null;
while (!open.IsEmpty())
{
current = open.Pop(); //Достаем наиболее оптимальную вершину
var x = current.Vert;
if (In(closed, x.Coord))
{
continue;
}
if (x.Coord == dest.Coord)
{
break; //return path
}
closed.Add(x.Coord);
foreach (var vert in CloserVertices(current.Vert.Coord))
{
if (!In(closed, vert.Coord))
{
var tmp = new RouteElem(vert, current.G + Vector3.Distance(current.Vert.Coord, vert.Coord), destination, current);
open.Add(tmp); //TODO: OPTIMIZE -> AddRange() - only 1 balance
}
}
}
if (current == null)
{
return(new List <Vector3>());
}
var last = current;
var path = new List <Vector3>();
var currElem = last;
while (currElem.PrevElem != null)
{
path.Add(currElem.Vert.Coord);
currElem = currElem.PrevElem;
}
path.Add(currElem.Vert.Coord);
path.Reverse();
return(path);
}
public List <Vector3> OLDFindRoute(Vector3 origin, Vector3 destination)
{
var heap = new MinHeap <RouteElem>(); //Хипа вместо приоритетой очереди
var orig = FindVertex(origin); //Находим вершину старт
var dest = FindVertex(destination); //Вершину, ближайшую к точку назначения
var closed = new List <Vector3>(); //Список пройденных вершин
if (orig == null || dest == null)
{
throw new ArithmeticException();
}
var first = new RouteElem(orig, 0, destination, null);
heap.Add(first); //Добавляем стартовый элемент в хипу
while (heap.Peek() != null && heap.Peek().Vert.Coord != dest.Coord) //Пока есть что смотреть и пока не дошли до точки назначения
//heap.PrintArr();
//Console.WriteLine("\n");
{
var current = heap.Pop(); //Достаем наиболее оптимальную вершину
//while(In(closed,current.Vert.Coord))
// current = heap.Pop();
//Console.WriteLine("Selected: "+current.Vert.Coord+" H:"+current.H+" F:"+current.F);
//heap.Remove(current);
//Console.Write("Popped: " + current.ToString()+"\n");
//heap.PrintArr();
//Console.WriteLine("\n");
foreach (var vert in current.Vert.AdjacentVertices()) //Проверяем все смежные вершины
//var condition = closed.Find(v => CompareVectors(v,vert.Coord)==0);
{
if (!In(closed, vert.Coord)) //Если вершина не была еще рассмотрена //Vector3 camparing. TODO: check
{
var tmp = new RouteElem(vert, current.G + Vector3.Distance(current.Vert.Coord, vert.Coord), destination, current); //Добавляем ее в хипу
heap.Add(tmp); //TODO: OPTIMIZE -> AddRange() - only 1 balance
}
}
//heap.Sort();
//list.Reverse();
closed.Add(current.Vert.Coord); //Добавляем рассматриваемую вершину в список пройденных
}
//путь
// Console.WriteLine("found");
Debug.LogWarning("CLOSED: " + closed.Count);
if (heap.Peek() == null) //Хипа пустая <==> путь не найден
{
return(new List <Vector3>()); //Пустой список
}
var last = heap.Peek();
var path = new List <Vector3>();
var currElem = last;
while (currElem.PrevElem != null)
{
path.Add(currElem.Vert.Coord);
currElem = currElem.PrevElem;
}
path.Add(currElem.Vert.Coord);
path.Reverse();
return(path);
//throw new NotImplementedException();
}
/**
* A* algorithm. Returns an empty list if there is no path.
* Returns a List, where the last element is the closest vertex to the destination point.
*
* @param origin
* @param destination
*/
public List <Vector3> FindRoute(Vector3 origin, Vector3 destination)
{
var open = new MinHeap <RouteElem>(); //Хипа вместо приоритетой очереди
var orig = FindVertex(origin); //Находим вершину старт
var dest = FindVertex(destination); //Вершину, ближайшую к точку назначения
var closed = new List <Vector3>(); //Список пройденных вершин
if (orig == null || dest == null)
{
throw new ArithmeticException();
}
var first = new RouteElem(orig, 0, destination, null);
open.Add(first); //Добавляем стартовый элемент в хипу
RouteElem current = null;
while (!open.IsEmpty())
{
current = open.Pop(); //Достаем наиболее оптимальную вершину
var x = current.Vert;
if (In(closed, x.Coord))
{
continue;
}
if (x.Coord == dest.Coord)
{
break; //return path
}
closed.Add(x.Coord);
foreach (var vert in current.Vert.AdjacentVertices()) //Проверяем все смежные вершины
//var condition = closed.Find(v => CompareVectors(v,vert.Coord)==0);
{
if (!In(closed, vert.Coord)) //Если вершина не была еще рассмотрена //Vector3 camparing. TODO: check
{
var tmp = new RouteElem(vert, current.G + Vector3.Distance(current.Vert.Coord, vert.Coord), destination, current); //Добавляем ее в хипу
open.Add(tmp); //TODO: OPTIMIZE -> AddRange() - only 1 balance
}
}
}
//путь
// Console.WriteLine("found");
//if(current == null)//open.Peek() == null) //Хипа пустая <==> путь не найден
// return new List<Vector3>(); //Пустой список
if ((int)current.Vert.Coord.x == 0 && (int)current.Vert.Coord.z == 0 && (int)destination.x != 0 && (int)destination.z != 0)
{
return(new List <Vector3>());
}
var last = current;//open.Peek();
var path = new List <Vector3>();
var currElem = last;
while (currElem.PrevElem != null)
{
path.Add(currElem.Vert.Coord);
currElem = currElem.PrevElem;
}
path.Add(currElem.Vert.Coord);
path.Reverse();
return(path);
//throw new NotImplementedException();
}