public void countTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
Assert.IsTrue(abiertos.count() == 0, "11) No es correcto, es " + abiertos.count() + " y debia ser " + 0);
Nodo nodo1 = new Nodo();
nodo1.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo1.coste = 10.0f;
Nodo nodo2 = new Nodo();
nodo2.vector = new Vector3(2.0f, 0.0f, 0.0f);
nodo2.coste = 1.0f;
abiertos.add(nodo1);
abiertos.add(nodo2);
Assert.IsTrue(abiertos.count() == 2, "12) No es correcto, es " + abiertos.count() + " y debia ser " + 2);
abiertos.getFirst();
Assert.IsTrue(abiertos.count() == 1, "13) No es correcto, es " + abiertos.count() + " y debia ser " + 1);
abiertos.delete(nodo1);
Assert.IsTrue(abiertos.count() == 0, "14) No es correcto, es " + abiertos.count() + " y debia ser " + 0);
}
public void getFirstNodoTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
Nodo comprobar_nodo;
Nodo nodo1 = new Nodo();
nodo1.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo1.coste = 10.0f;
Nodo nodo2 = new Nodo();
nodo2.vector = new Vector3(2.0f, 0.0f, 0.0f);
nodo2.coste = 1.0f;
abiertos.add(nodo1);
abiertos.add(nodo2);
comprobar_nodo = abiertos.getFirst();
Assert.IsTrue(comprobar_nodo.coste == nodo2.coste && comprobar_nodo.vector == nodo2.vector, "10) No es el nodo que debia ser el primero");
}
public void getEmptyTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
Nodo nodo1 = new Nodo();
nodo1.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo1.coste = 10.0f;
Nodo nodo2 = new Nodo();
nodo2.vector = new Vector3(2.0f, 0.0f, 0.0f);
nodo2.coste = 5.0f;
abiertos.add(nodo1);
abiertos.add(nodo2);
abiertos.getEmpty();
Assert.IsTrue(abiertos.count() == 0, "16) No esta vacio");
}
public void addNodoTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
Nodo nodo = new Nodo();
nodo.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo.coste = 1.0f;
abiertos.add(nodo);
Nodo comprobar = abiertos.getFirst();
Assert.IsTrue(comprobar.coste == nodo.coste && comprobar.vector == nodo.vector, "1) No es el mismo nodo que introducimos");
}
public void comprobarTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
Nodo nodo = new Nodo();
nodo.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo.coste = 1.0f;
abiertos.add(nodo);
bool comprobar = abiertos.comprobar(nodo);
Assert.IsTrue(comprobar, "2) No esta el nodo que introducimos");
}
public override void iniciarCalcularRuta(Vector3 v_inicio, Vector3 v_meta, float v_angulo_coche, ObtenerMapa v_mapa, Parrilla v_parrilla, float p_peso, int tam_parrilla, int ancho, int largo, bool v_dibujar_casillas)
{
Vector3[] array_vertices;
dibujar_casillas = v_dibujar_casillas;
//cerrados = new Cerrados (ancho, largo);
cerrados = new Cerrados();
sucesores = new List <Nodo> ();
abiertos = new Abiertos(tam_parrilla, ancho, largo);
//abiertos = new Abiertos (tam_parrilla);
peso = p_peso;
abiertos.getEmpty();
cerrados.getEmpty();
vector_inicio = v_inicio;
vector_meta = v_meta;
mapa = v_mapa;
parrilla = v_parrilla;
vertices = new HashSet <Vector3> ();
array_vertices = mapa.getVertices();
foreach (Vector3 vertice in array_vertices)
{
vertices.Add(vertice);
}
vertices.Add(vector_meta);
meta_encontrada = false;
nodo_final = null;
nodo_actual = null;
nodo_inicio = new Nodo();
nodo_inicio.vector = vector_inicio;
nodo_inicio.padre = null;
nodo_inicio.coste = 0;
abiertos.add(nodo_inicio);
}
public void deleteTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
bool comprobar;
Nodo nodo = new Nodo();
nodo.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo.coste = 1.0f;
abiertos.add(nodo);
comprobar = abiertos.comprobar(nodo);
Assert.IsTrue(comprobar, "3) No esta el nodo que introducimos");
comprobar = abiertos.delete(nodo);
Assert.IsTrue(comprobar, "4) Se ha borrado el nodo");
comprobar = abiertos.comprobar(nodo);
Assert.IsFalse(comprobar, "5) No se ha borrado");
}
public void findNodoTest()
{
Abiertos abiertos = new Abiertos(9, 3, 3);
bool comprobar;
Nodo comprobar_nodo;
Nodo nodo = new Nodo();
nodo.vector = new Vector3(1.0f, 0.0f, 0.0f);
nodo.coste = 1.0f;
abiertos.add(nodo);
comprobar = abiertos.find(nodo, out comprobar_nodo);
Assert.IsTrue(comprobar_nodo.coste == nodo.coste && comprobar_nodo.vector == nodo.vector, "6) No es el mismo nodo que introducimos");
Assert.IsTrue(comprobar, "7) No se ha encontrado el nodo que introducimos");
abiertos.delete(nodo);
comprobar = abiertos.find(nodo, out comprobar_nodo);
Assert.IsNull(comprobar_nodo, "8) No tenia que haber encontrado el nodo");
Assert.IsFalse(comprobar, "9) No tenia que haber encontrado el nodo");
}
public override void iniciarCalcularRuta(Vector3 v_inicio, Vector3 v_meta, float v_angulo_coche, ObtenerMapa v_mapa, Parrilla v_parrilla, float p_peso, int tam_parrilla, int v_ancho, int v_largo, bool v_dibujar_casillas)
{
Vector3[] array_vertices;
dibujar_casillas = v_dibujar_casillas;
ancho = v_ancho;
largo = v_largo;
//cerrados = new Cerrados (ancho, largo);
cerrados = new Cerrados();
sucesores = new List <Nodo> ();
abiertos = new Abiertos(tam_parrilla, ancho, largo);
//abiertos = new Abiertos (tam_parrilla);
peso = p_peso;
abiertos.getEmpty();
cerrados.getEmpty();
vector_inicio = v_inicio;
vector_meta = v_meta;
mapa = v_mapa;
parrilla = v_parrilla;
vertices = new HashSet <Vector3> ();
array_vertices = mapa.getVertices();
foreach (Vector3 vertice in array_vertices)
{
vertices.Add(vertice);
}
vertices.Add(vector_meta);
meta_encontrada = false;
nodo_final = null;
nodo_actual = null;
nodo_inicio = new Nodo();
nodo_inicio.vector = vector_inicio;
nodo_inicio.vector_hybrid = vector_inicio;
nodo_inicio.angulo_hybrid = v_angulo_coche;
nodo_inicio.padre = null;
nodo_inicio.coste = 0;
abiertos.add(nodo_inicio);
mapa.setRadio(0.5f); //
array_mapa_obstaculos = new int[ancho + 1, largo + 1, 1];
tiempo_inicio = Time.realtimeSinceStartup;
crearMapaObstaculos();
tiempo_final = Time.realtimeSinceStartup;
Debug.Log("Mapa de obstaculos creado en: " + (tiempo_final - tiempo_inicio) + " segs.");
tiempo_inicio = Time.realtimeSinceStartup;
array_mapa_distancias = crearMapaDistancias();
tiempo_final = Time.realtimeSinceStartup;
Debug.Log("Mapa de Distancias creado en: " + (tiempo_final - tiempo_inicio) + " segs.");
tiempo_inicio = Time.realtimeSinceStartup;
array_heuristica_obstaculos = crearMapaHeuristicaObstaculos();
tiempo_final = Time.realtimeSinceStartup;
Debug.Log("Mapa de heuristica de obstaculos creado en: " + (tiempo_final - tiempo_inicio) + " segs.");
radio_giro_rad = ((Constantes.distancia / Constantes.coche_largo) * Mathf.Tan(Mathf.Deg2Rad * Constantes.coche_max_angulo));
radio_giro = Mathf.Rad2Deg * radio_giro_rad;
Debug.Log("Radio giro rad: " + radio_giro_rad);
Debug.Log("Radio giro: " + radio_giro);
}
public override bool pasoCalcularRuta(out bool error)
{
error = false;
if (abiertos.count() > 0 && !meta_encontrada)
{
nodo_actual = abiertos.getFirst();
if (esMeta(nodo_actual, vector_meta))
{
meta_encontrada = true;
nodo_final = nodo_actual;
v_trayectoria = vectoresCamino(nodo_final);
n_trayectoria = nodosCamino(nodo_final);
}
else
{
cerrados.add(nodo_actual);
if (dibujar_casillas)
{
parrilla.visualizarCasilla(nodo_actual.vector, Constantes._CERRADOS);
}
sucesores = CalcularSucesores(nodo_actual, vector_meta, mapa);
foreach (Nodo sucesor in sucesores)
{
Nodo anterior;
if (abiertos.find(sucesor, out anterior))
{
if (anterior.coste > sucesor.coste)
{
anterior.padre = nodo_actual;
anterior.coste = sucesor.coste;
anterior.costeG = sucesor.costeG;
anterior.costeH = sucesor.costeH;
//Para el hybrid
anterior.sentido = sucesor.sentido;
anterior.vector_hybrid = sucesor.vector_hybrid;
anterior.angulo_hybrid = sucesor.angulo_hybrid;
abiertos.updatePrioridad(anterior, anterior.coste);
}
}
else
{
if (cerrados.find(sucesor, out anterior))
{
if (anterior.coste > sucesor.coste)
{
cerrados.delete(anterior);
abiertos.add(sucesor);
if (dibujar_casillas)
{
parrilla.visualizarCasilla(sucesor.vector, Constantes._ABIERTOS);
}
}
}
else //No esta ni en abiertos ni en cerrados
{
abiertos.add(sucesor);
if (dibujar_casillas)
{
parrilla.visualizarCasilla(sucesor.vector, Constantes._ABIERTOS);
}
}
}
}
}
}
else //abiertos esta vacio
{
if (abiertos.count() == 0)
{
error = true;
}
}
return(meta_encontrada);
}
