internal NodoGrafoAStar pop()
{
NodoGrafoAStar ret = (NodoGrafoAStar)lista[0];
lista.RemoveAt(0);
return(ret);
}
protected internal void addOrReplace(NodoGrafoAStar nuevoNodo)
{
NodoGrafoAStar posibleaASustituir = null;
bool estaEnListaOpen = false;
int index = 0;
foreach (NodoGrafoAStar noditoOpen in lista)
{
if (nuevoNodo.posicionGrid == noditoOpen.posicionGrid)
{
estaEnListaOpen = true;
if (noditoOpen.totalCost > nuevoNodo.totalCost)
{
posibleaASustituir = noditoOpen;
}
break;
}
index++;
}
if (posibleaASustituir != null)
{
lista.RemoveAt(index);
lista.Insert(index, nuevoNodo);
}
else if (!estaEnListaOpen)
{
lista.Add(nuevoNodo);
}
}
private static int getDistance(NodoGrafoAStar origin, Vector2 destiny)
{
int dstX = (int)Mathf.Abs(origin.posicionGrid.x - destiny.x);
int dstY = (int)Mathf.Abs(origin.posicionGrid.y - destiny.y);
if (dstX > dstY)
{
return(14 * dstY + 10 * (dstX - dstY)); //el 14 y el 10 depende del tamaño de nuestro grid
}
return(14 * dstX + 10 * (dstY - dstX));
}
public AStarSD(StatsInfo.TIPO_TERRENO[][] terrenos, Vector2 origen, Vector2 destino)
{
this.terrenos = terrenos;
this.origen = origen;
this.destino = destino;/*
* distancias = new float[terrenos.Length][];
* for (int i=0; i < distancias.Length; i++)
* {
* distancias[i] = new float[terrenos[i].Length];
* }*/
float estimatedCost = (destino - origen).magnitude;
nodoOrigen = new NodoGrafoAStar(origen, (destino - origen).magnitude, 0f, null);
}
protected internal void add(NodoGrafoAStar nodo)
{
int index = 0;
foreach (NodoGrafoAStar nodito in lista)
{
if (nodito.totalCost > nodo.totalCost)
{
break;
}
index++;
}
if (index >= 0 && index < lista.Count)
{
lista.Insert(index, nodo);
}
else
{
lista.Add(nodo);
}
}
public LinkedList <NodoGrafoAStar> calcularAdyacentes(NodoGrafoAStar actual, StatsInfo.TIPO_PERSONAJE type)
{
LinkedList <NodoGrafoAStar> listanodos = new LinkedList <NodoGrafoAStar>();
for (int i = -1; i < 2; i++)
{
for (int j = -1; j < 2; j++)
{
if (i != 0 || j != 0)
{
Vector2 newPosi = new Vector2(actual.posicionGrid.x + i, actual.posicionGrid.y + j);
if (terrenos[(int)newPosi.x][(int)newPosi.y] != StatsInfo.TIPO_TERRENO.INFRANQUEABLE)
{
float inversaVelocidad = 1 / StatsInfo.velocidadUnidadPorTerreno[(int)terrenos[(int)newPosi.x][(int)newPosi.y]][(int)type];
float newG = actual.costFromOrigin + (destino - newPosi).magnitude * inversaVelocidad;
listanodos.AddLast(new NodoGrafoAStar(newPosi, (destino - newPosi).magnitude, newG, actual));
}
}
}
}
return(listanodos);
}
private static LinkedList <NodoGrafoAStar> calcularAdyacentes(NodoGrafoAStar actual, Vector2 destino, NodoGrafoAStar[][] nodosUsados, StatsInfo.TIPO_PERSONAJE type, bool team)
{
LinkedList <NodoGrafoAStar> listanodos = new LinkedList <NodoGrafoAStar>();
for (int i = -1; i < 2; i++)
{
for (int j = -1; j < 2; j++)
{
//8 vecinos
if (i != 0 || j != 0)
//4 vecinos
//if (System.Math.Abs(i) != System.Math.Abs(j))
{
Vector2 newPosi = new Vector2(actual.posicionGrid.x + i, actual.posicionGrid.y + j);
if (terrenos[(int)newPosi.x][(int)newPosi.y] != StatsInfo.TIPO_TERRENO.INFRANQUEABLE)
{
NodoGrafoAStar nodoActual = nodosUsados[(int)newPosi.x][(int)newPosi.y];
if (nodoActual != null)
{
listanodos.AddLast(nodoActual);
}
else
{
float inversaVelocidad = 1 / StatsInfo.velocidadUnidadPorTerreno[(int)terrenos[(int)newPosi.x][(int)newPosi.y]][(int)type];
float newG = (int)(actual.costFromOrigin + (actual.posicionGrid - newPosi).magnitude * inversaVelocidad);
float influencePenalty = calculateInfluencePenalty(team, newPosi);
float terrainPenalty = inversaVelocidad * 70;
NodoGrafoAStar nuevoNodo = new NodoGrafoAStar(newPosi, getDistance(newPosi, destino) + influencePenalty + terrainPenalty, newG, actual);
listanodos.AddLast(nuevoNodo);
nodosUsados[(int)newPosi.x][(int)newPosi.y] = nuevoNodo;
}
}
}
}
}
return(listanodos);
}
internal static List <Vector2> aStarPath(Vector2 origen, Vector2 end, StatsInfo.TIPO_PERSONAJE tipo, bool team)
{
NodoGrafoAStar nodoOrigen = new NodoGrafoAStar(origen, (end - origen).magnitude, 0f, null);
LinkedList <Vector2> recorrido = new LinkedList <Vector2>();
float estimatedCost = (end - origen).magnitude;
if (origen == end || terrenos[(int)end.x][(int)end.y] == StatsInfo.TIPO_TERRENO.INFRANQUEABLE)
{
return(new List <Vector2>());
}
NodoGrafoAStar[][] nodos = new NodoGrafoAStar[165][];
for (int i = 0; i < nodos.Length; i++)
{
nodos[i] = new NodoGrafoAStar[65];
}
HashSet <Vector2> closedPositions = new HashSet <Vector2>();
Heap <NodoGrafoAStar> openPositions = new Heap <NodoGrafoAStar>(maxHeapSize);
openPositions.Add(nodoOrigen);
NodoGrafoAStar nodoActual = null;
while (openPositions.Count > 0)
{
nodoActual = openPositions.RemoveFirst();
closedPositions.Add(nodoActual.posicionGrid);
if (nodoActual.posicionGrid == end)
{
break;
}
LinkedList <NodoGrafoAStar> adyacentes = calcularAdyacentes(nodoActual, end, nodos, tipo, team);
//LinkedList<NodoGrafoAStar> adyacentesFiltrados
foreach (NodoGrafoAStar nodito in adyacentes)
{
//if closed.contains(neighbour)
bool estaEnListaClosed = closedPositions.Contains(nodito.posicionGrid);
if (estaEnListaClosed)
{
continue;
}
//calculamos distancia al siguiente nodo desde el que estamos
float inversaVelocidad = 1 / StatsInfo.velocidadUnidadPorTerreno[(int)terrenos[(int)nodito.posicionGrid.x][(int)nodito.posicionGrid.y]][(int)tipo];
//float newG = nodoActual.costFromOrigin + (nodoActual.posicionGrid - nodito.posicionGrid).magnitude * inversaVelocidad;
float newG = (int)(nodoActual.costFromOrigin + Mathf.RoundToInt((nodoActual.posicionGrid - nodito.posicionGrid).magnitude * inversaVelocidad));
if (newG < nodito.costFromOrigin || (nodos[(int)nodito.posicionGrid.x][(int)nodito.posicionGrid.y] != null && !openPositions.Contains(nodito)))
{
nodito.costFromOrigin = newG;
//nodito.estimatedCost = (end-nodito.posicionGrid).magnitude;
float influencePenalty = calculateInfluencePenalty(team, nodito.posicionGrid);
float terrainPenalty = inversaVelocidad * 70;
nodito.estimatedCost = getDistance(nodito, end) + influencePenalty + terrainPenalty;
nodito.padre = nodoActual;
if (nodos[(int)nodito.posicionGrid.x][(int)nodito.posicionGrid.y] != null && !openPositions.Contains(nodito))
{
openPositions.Add(nodito);
}
else
{
openPositions.UpdateItem(nodito);
}
}
}
}
//Calculamos el camino a seguir en base a los padres del nodo destino
NodoGrafoAStar aux = nodoActual;
while (aux.padre != null)
{
recorrido.AddFirst(aux.posicionGrid);
aux = aux.padre;
}
return(new List <Vector2>(recorrido));
}
protected internal override Steering getSteering(PersonajeBase personaje)
{
if (!setupAEstrella)
{
LinkedList <NodoGrafoAStar> closedPositions = new LinkedList <NodoGrafoAStar>();
closedPositions.AddLast(nodoOrigen);
LinkedList <NodoGrafoAStar> openPositions = new LinkedList <NodoGrafoAStar>();
NodoGrafoAStar nodoActual = nodoOrigen;
while (!setupAEstrella)
{
LinkedList <NodoGrafoAStar> adyacentes = calcularAdyacentes(nodoActual, personaje.tipo);
//LinkedList<NodoGrafoAStar> adyacentesFiltrados
foreach (NodoGrafoAStar nodito in adyacentes)
{
//Observamos lista closed
bool estaEnListaClosed = false;
foreach (NodoGrafoAStar noditoClosed in closedPositions)
{
//Si el nodo ya está en la lista closed, no se considera
if (noditoClosed.posicionGrid == nodito.posicionGrid)
{
estaEnListaClosed = true;
break;
}
}
if (estaEnListaClosed)
{
continue;
}
//Observamos lista open
NodoGrafoAStar posibleaASustituir = null;
bool estaEnListaOpen = false;
foreach (NodoGrafoAStar noditoOpen in openPositions)
{
if (nodito.posicionGrid == noditoOpen.posicionGrid)
{
estaEnListaOpen = true;
if (noditoOpen.totalCost > nodito.totalCost)
{
posibleaASustituir = noditoOpen;
}
break;
}
}
if (posibleaASustituir != null)
{
openPositions.Remove(posibleaASustituir);
openPositions.AddLast(nodito);
}
else if (!estaEnListaOpen)
{
openPositions.AddLast(nodito);
}
}
//Calculamos siguiente nodo
float minorCost = float.MaxValue;
NodoGrafoAStar next = null;
foreach (NodoGrafoAStar noditoOpen in openPositions)
{
if (noditoOpen.totalCost < minorCost)
{
minorCost = noditoOpen.totalCost;
next = noditoOpen;
}
}
nodoActual = next;
openPositions.Remove(nodoActual);
closedPositions.AddLast(nodoActual);
//Comprobacion de parada(llegamos al destina)
foreach (NodoGrafoAStar noditoClosed in closedPositions)
{
if (noditoClosed.posicionGrid == destino)
{
setupAEstrella = true;
}
}
}
//Calculamos el camino a seguir en base a los padres del nodo destino
NodoGrafoAStar aux = nodoActual;
while (aux.padre != null)
{
recorrido.AddFirst(aux.posicionGrid);
aux = aux.padre;
}
}
if (pursue.finishedLinear || !setupRecorrido)
{
if (pasoActual >= recorrido.Count - 1)
{
_finishedLinear = true;
_finishedAngular = true;
return(new Steering());
}
else
{
pasoActual++;
personaje.fakeMovement.posicion = SimManagerFinal.gridToPosition(recorrido.ElementAt(pasoActual));
personaje.fakeMovement.moveTo(SimManagerFinal.gridToPosition(recorrido.ElementAt(pasoActual)));
pursue.target = personaje.fakeMovement;
setupRecorrido = true;
return(pursue.getSteering(personaje));
}
}
else
{
return(pursue.getSteering(personaje));
}
}
