public void HybridAEstrellaMapaDistancias()
{
AlgoritmoRuta hybrid = new Hybrid_a_estrella();
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Vector3 inicio = new Vector3(20.0f, 0.0f, 0.0f);
Vector3 meta = new Vector3(0.0f, 0.0f, 0.0f);
float angulo = coche.transform.rotation.eulerAngles.y;
ObtenerMapa mapa = new ObtenerMapa();
Parrilla parrilla = new Parrilla(new GameObject(), new GameObject());
hybrid.iniciarCalcularRuta(inicio, meta, angulo, mapa, parrilla, 0.0f, 100 * 100, 100, 100, false);
int[,,] m_obstaculos = hybrid.getMapaDistancias();
/*
* Hay obstaculos en:
* 0, 0 ,0
* -31, -0, 25
* 27, 0, 30
*
* No hay en:
* 0,0, 45
* 0,0, -45
*
* hay que pasarlos a indices sumandoles la mitad del ancho (50)
*/
Assert.IsTrue(m_obstaculos [50, 50, 0] == 0, "9) Es obstaculos la distancia debia ser 0, y es " + m_obstaculos [50, 50, 0]);
Assert.IsTrue(m_obstaculos[19, 75, 0] == 0, "10) Es obstaculos la distancia debia ser 0, y es " + m_obstaculos [19, 75, 0]);
Assert.IsTrue(m_obstaculos[57, 80, 0] == 0, "11) Es obstaculos la distancia debia ser 0, y es " + m_obstaculos [57, 80, 0]);
Assert.IsTrue(m_obstaculos[50, 95, 0] == 30, "12) La distancia debia ser 30, y es " + m_obstaculos [50, 95, 0]);
Assert.IsTrue(m_obstaculos[50, 5, 0] == 10, "13) La distancia debia ser 10, y es " + m_obstaculos [50, 5, 0]);
}
protected override List <Nodo> CalcularSucesores(Nodo n_actual, Vector3 meta, ObtenerMapa mapa)
{
List <Nodo> sucesores = new List <Nodo> ();
foreach (Vector3 vector in vertices)
{
// La version comprobando las linea de vision en los dos sentidos no funciona bien con los vertices
// porque no encuentra los mejores caminos
//if (mapa.lineaVision (n_actual.vector, vector) && mapa.lineaVision (vector, n_actual.vector)) {
if (mapa.lineaVision(n_actual.vector, vector))
{
Nodo nuevo_sucesor = new Nodo();
nuevo_sucesor.vector = vector;
nuevo_sucesor.padre = n_actual;
nuevo_sucesor.costeG = funcionG(nuevo_sucesor);
nuevo_sucesor.costeH = funcionH(nuevo_sucesor, meta);
nuevo_sucesor.coste = (peso * nuevo_sucesor.costeG) + nuevo_sucesor.costeH;
sucesores.Add(nuevo_sucesor);
}
}
return(sucesores);
}
public void HybridAEstrellaMapaObstaculosTest()
{
AlgoritmoRuta hybrid = new Hybrid_a_estrella();
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Vector3 inicio = new Vector3(20.0f, 0.0f, 0.0f);
Vector3 meta = new Vector3(0.0f, 0.0f, 0.0f);
float angulo = coche.transform.rotation.eulerAngles.y;
ObtenerMapa mapa = new ObtenerMapa();
Parrilla parrilla = new Parrilla(new GameObject(), new GameObject());
hybrid.iniciarCalcularRuta(inicio, meta, angulo, mapa, parrilla, 0.0f, 100 * 100, 100, 100, false);
int[,,] m_obstaculos = hybrid.getMapaObstaculos();
/*
* Hay obstaculos en:
* 0, 0 ,0
* -31, -0, 25
* 27, 0, 30
*
* No hay en:
* 0,0, 45
* 0,0, -45
*
* hay que pasarlos a indices sumandoles la mitad del ancho (50)
*/
Assert.IsTrue(m_obstaculos [50, 50, 0] == Constantes._OBSTACULO, "4) No hay obstaculo pero debia haberlo");
Assert.IsTrue(m_obstaculos[19, 75, 0] == Constantes._OBSTACULO, "5) No hay obstaculo pero debia haberlo");
Assert.IsTrue(m_obstaculos[57, 80, 0] == Constantes._OBSTACULO, "6) No hay obstaculo pero debia haberlo");
Assert.IsTrue(m_obstaculos[50, 95, 0] == Constantes._LIBRE, "7) No es espacio libre pero debia serlo");
Assert.IsTrue(m_obstaculos[50, 5, 0] == Constantes._LIBRE, "8) No es espacio libre pero debia serlo");
}
public void HybridAEstrellaTestsHayRutaTest()
{
AlgoritmoRuta hybrid = new Hybrid_a_estrella();
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Vector3 inicio = new Vector3(44.0f, 0.0f, 0.0f);
Vector3 meta = new Vector3(44.0f, 0.0f, 10.0f);
float angulo = coche.transform.rotation.eulerAngles.y;
ObtenerMapa mapa = new ObtenerMapa();
Parrilla parrilla = new Parrilla(new GameObject(), new GameObject());
Nodo[] trayectoria;
bool error = false;
hybrid.iniciarCalcularRuta(inicio, meta, angulo, mapa, parrilla, 0.0f, 100 * 100, 100, 100, false);
while (!hybrid.pasoCalcularRuta(out error) && !error)
{
}
Assert.IsFalse(error, "1) No ha encontrado una ruta");
trayectoria = hybrid.getTrayectoriaNodos();
float distancia = Vector3.Distance(trayectoria [trayectoria.Length - 1].vector_hybrid, meta);
Assert.IsTrue(distancia < 2, "2) No ha llegado a la meta. Es " + trayectoria [trayectoria.Length - 1].vector_hybrid + " y debia ser " + meta);
}
public void esRecorribleTest()
{
ObtenerMapa mapa = new ObtenerMapa();
bool comprobar;
Vector3 posicion;
mapa.setRadio(0.5f);
posicion = new Vector3(45.0f, 0.0f, 0.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsTrue(comprobar, "1) No es recorrible y debia serlo.");
posicion = new Vector3(45.0f, 0.0f, 40.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsTrue(comprobar, "2) No es recorrible y debia serlo.");
posicion = new Vector3(-45.0f, 0.0f, -40.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsTrue(comprobar, "3) No es recorrible y debia serlo.");
posicion = new Vector3(0.0f, 0.0f, 0.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsFalse(comprobar, "4) No es obstaculo y debia serlo.");
posicion = new Vector3(-31.0f, 0.0f, -25.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsFalse(comprobar, "5) No es obstaculo y debia serlo.");
posicion = new Vector3(-4.0f, 0.0f, -38.0f);
comprobar = mapa.esRecorrible(posicion);
Assert.IsFalse(comprobar, "6) No es obstaculo y debia serlo.");
}
public void lineaVisionTest()
{
ObtenerMapa mapa = new ObtenerMapa();
bool comprobar;
Vector3 posicion1;
Vector3 posicion2;
mapa.setRadio(0.5f);
posicion1 = new Vector3(45.0f, 0.0f, 0.0f);
posicion2 = new Vector3(0.0f, 0.0f, 0.0f);
comprobar = mapa.lineaVision(posicion1, posicion2);
Assert.IsFalse(comprobar, "7) No deberia haber linea de vision.");
posicion1 = new Vector3(45.0f, 0.0f, 40.0f);
posicion2 = new Vector3(-31.0f, 0.0f, -25.0f);
comprobar = mapa.lineaVision(posicion1, posicion2);
Assert.IsFalse(comprobar, "8) No deberia haber linea de vision.");
posicion1 = new Vector3(-45.0f, 0.0f, -40.0f);
posicion2 = new Vector3(-4.0f, 0.0f, -38.0f);
comprobar = mapa.lineaVision(posicion1, posicion2);
Assert.IsFalse(comprobar, "9) No deberia haber linea de vision.");
posicion1 = new Vector3(-45.0f, 0.0f, 45.0f);
posicion2 = new Vector3(45.0f, 0.0f, 45.0f);
comprobar = mapa.lineaVision(posicion1, posicion2);
Assert.IsTrue(comprobar, "9) No deberia haber obstaculos en el camino.");
posicion1 = new Vector3(45.0f, 0.0f, 0.0f);
posicion2 = new Vector3(45.0f, 0.0f, 40.0f);
comprobar = mapa.lineaVision(posicion1, posicion2);
Assert.IsTrue(comprobar, "10) No deberia haber obstaculos en el camino.");
}
public void aEstrellaVerticesHayRutaTest()
{
AlgoritmoRuta astar_vertices = new A_estrella_vertices();
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Vector3 inicio = new Vector3(44.0f, 0.0f, 0.0f);
Vector3 meta = new Vector3(44.0f, 0.0f, 10.0f);
float angulo = coche.transform.rotation.eulerAngles.y;
ObtenerMapa mapa = new ObtenerMapa();
Parrilla parrilla = new Parrilla(new GameObject(), new GameObject());
Vector3[] trayectoria;
bool error = false;
astar_vertices.iniciarCalcularRuta(inicio, meta, angulo, mapa, parrilla, 1.0f, 100 * 100, 100, 100, false);
while (!astar_vertices.pasoCalcularRuta(out error) && !error)
{
}
Assert.IsFalse(error, "1) No ha encontrado una ruta");
trayectoria = astar_vertices.getTrayectoria();
Assert.IsTrue(trayectoria [trayectoria.Length - 1] == meta, "2) No ha llegado a la meta. Es " + trayectoria [trayectoria.Length - 1] + " y debia ser " + meta);
}
protected virtual List <Nodo> CalcularSucesores(Nodo n_actual, Vector3 meta, ObtenerMapa mapa)
{
List <Nodo> sucesores = new List <Nodo> ();
const int num_sucesores = 8; //las 8 direcciones posibles
Vector3[] movimientos = new Vector3[num_sucesores] {
new Vector3(0.0f, 0.0f, 1.0f), // adelante
new Vector3(-1.0f, 0.0f, 1.0f), // adelante derecha
new Vector3(-1.0f, 0.0f, 0.0f), // derecha
new Vector3(-1.0f, 0.0f, -1.0f), // atras derecha
new Vector3(0.0f, 0.0f, -1.0f), // atras
new Vector3(1.0f, 0.0f, -1.0f), // atras izquierda
new Vector3(1.0f, 0.0f, 0.0f), // izquiera
new Vector3(1.0f, 0.0f, 1.0f) // adelante izquierda
};
Nodo[] sucesor = rellenarSucesores(num_sucesores, n_actual, movimientos);
sucesores = SucesoresValidos(sucesor, mapa);
foreach (Nodo sucesor_valido in sucesores)
{
sucesor_valido.costeG = funcionG(sucesor_valido);
sucesor_valido.costeH = funcionH(sucesor_valido, meta);
sucesor_valido.coste = (peso * sucesor_valido.costeG) + sucesor_valido.costeH;
}
return(sucesores);
}
public PathSmoothing(ObtenerMapa p_mapa, Vector3[] p_trayectoria, float p_peso_trayectoria, float p_peso_suavizado)
{
mapa = p_mapa;
trayectoria = p_trayectoria;
peso_trayectoria = p_peso_trayectoria;
peso_suavizado = p_peso_suavizado;
tolerancia = Constantes.ps_tolerancia;
num_puntos_bezier = Constantes.ps_num_puntos_bezier;
}
// comprueba que los posibles sucesores esten dentro del rango posible
protected override List <Nodo> SucesoresValidos(Nodo[] sucesor, ObtenerMapa mapa)
{
List <Nodo> sucesores = new List <Nodo> ();
foreach (Nodo sucesor_valido in sucesor)
{
if (mapa.lineaVision(sucesor_valido.padre.vector_hybrid, sucesor_valido.vector_hybrid))
{
sucesores.Add(sucesor_valido);
}
}
return(sucesores);
}
// comprueba que los posibles sucesores esten dentro del rango posible
protected virtual List <Nodo> SucesoresValidos(Nodo[] sucesor, ObtenerMapa mapa)
{
List <Nodo> sucesores = new List <Nodo> ();
foreach (Nodo sucesor_valido in sucesor)
{
//Comprobamos las dos direcciones porque hemos encontrado que no siempre, aunque deberia, da el mismo resultado
//Asi evitamos bugs debido a que sea visible/alcanzable desde una direccion pero no desde la otra
if (mapa.lineaVision(sucesor_valido.padre.vector, sucesor_valido.vector) && mapa.lineaVision(sucesor_valido.vector, sucesor_valido.padre.vector))
{
//if ( mapa.lineaVision (sucesor_valido.padre.vector, sucesor_valido.vector)){
sucesores.Add(sucesor_valido);
}
}
return(sucesores);
}
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 curvaBezierTest2()
{
Vector3[] trayectoria_prueba = new Vector3[7];
Vector3[] resultado_prueba;
ObtenerMapa mapa = new ObtenerMapa();
Vector3 comprobar = new Vector3(44.8f, 0.0f, 0.2f);
trayectoria_prueba [0] = new Vector3(45.0f, 0.0f, 0.0f);
trayectoria_prueba [1] = new Vector3(44.0f, 0.0f, 1.0f);
trayectoria_prueba [2] = new Vector3(43.0f, 0.0f, 2.0f);
PathSmoothing pruebaPS = new PathSmoothing(mapa, trayectoria_prueba, 0.5f, 0.1f);
resultado_prueba = pruebaPS.curvaBezier(trayectoria_prueba);
Assert.IsTrue(resultado_prueba[1] == comprobar, "NO son iguales: es " + resultado_prueba[1] + " | debia ser: " + comprobar);
}
public void aEstrellaVerticesNoRutaTest()
{
AlgoritmoRuta astar_vertices = new A_estrella_vertices();
GameObject coche = GameObject.FindGameObjectWithTag("Coche");
Vector3 inicio = new Vector3(20.0f, 0.0f, 0.0f);
Vector3 meta = new Vector3(0.0f, 0.0f, 0.0f);
float angulo = coche.transform.rotation.eulerAngles.y;
ObtenerMapa mapa = new ObtenerMapa();
Parrilla parrilla = new Parrilla(new GameObject(), new GameObject());
bool error = false;
astar_vertices.iniciarCalcularRuta(inicio, meta, angulo, mapa, parrilla, 1.0f, 100 * 100, 100, 100, false);
while (!astar_vertices.pasoCalcularRuta(out error) && !error)
{
}
Assert.IsTrue(error, "3) No ha devuelto error cuando no hay ruta");
}
public void eliminarZigZagTest()
{
Vector3[] trayectoria_prueba = new Vector3[7];
Vector3[] resultado_prueba;
ObtenerMapa mapa = new ObtenerMapa();
trayectoria_prueba [0] = new Vector3(45.0f, 0.0f, 0.0f);
trayectoria_prueba [1] = new Vector3(45.0f, 0.0f, 1.0f);
trayectoria_prueba [2] = new Vector3(45.0f, 0.0f, 2.0f);
trayectoria_prueba [3] = new Vector3(45.0f, 0.0f, 3.0f);
trayectoria_prueba [4] = new Vector3(45.0f, 0.0f, 4.0f);
trayectoria_prueba [5] = new Vector3(45.0f, 0.0f, 5.0f);
trayectoria_prueba [6] = new Vector3(45.0f, 0.0f, 6.0f);
PathSmoothing pruebaPS = new PathSmoothing(mapa, trayectoria_prueba, 0.5f, 0.1f);
resultado_prueba = pruebaPS.eliminarZigZag(trayectoria_prueba);
Assert.IsTrue(resultado_prueba.Length == 2, "NO son iguales: es " + resultado_prueba.Length + " | debia ser: " + 2);
}
public void curvaBezierTest1()
{
Vector3[] trayectoria_prueba = new Vector3[7];
Vector3[] resultado_prueba;
ObtenerMapa mapa = new ObtenerMapa();
int comprobar = Mathf.RoundToInt((3 * Constantes.ps_num_puntos_bezier) + 4);
trayectoria_prueba [0] = new Vector3(45.0f, 0.0f, 0.0f);
trayectoria_prueba [1] = new Vector3(45.0f, 0.0f, 1.0f);
trayectoria_prueba [2] = new Vector3(45.0f, 0.0f, 2.0f);
trayectoria_prueba [3] = new Vector3(45.0f, 0.0f, 3.0f);
trayectoria_prueba [4] = new Vector3(45.0f, 0.0f, 4.0f);
trayectoria_prueba [5] = new Vector3(45.0f, 0.0f, 5.0f);
trayectoria_prueba [6] = new Vector3(45.0f, 0.0f, 6.0f);
PathSmoothing pruebaPS = new PathSmoothing(mapa, trayectoria_prueba, 0.5f, 0.1f);
resultado_prueba = pruebaPS.curvaBezier(trayectoria_prueba);
Assert.IsTrue(resultado_prueba.Length == comprobar, "NO son iguales: es " + resultado_prueba.Length + " | debia ser: " + comprobar);
}
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);
}
protected override List <Nodo> CalcularSucesores(Nodo n_actual, Vector3 meta, ObtenerMapa mapa)
{
//float radio_giro_rad = ((Constantes.distancia / Constantes.coche_largo) * Mathf.Tan (Mathf.Deg2Rad * Constantes.coche_max_angulo));
//float radio_giro = Mathf.Rad2Deg * radio_giro_rad;
//Habra 6 sucesores: adelante, retroceder
// adelante giro izquierda, adelante giro derecha
// retroceder giro izquierda, retroceder giro derecha
Nodo[] sucesores_array = new Nodo[6];
Nodo[] sucesores_validos;
List <Nodo> sucesores_discretos = new List <Nodo>();
List <Nodo> sucesores_seleccionados = new List <Nodo>();
//List <Nodo> final = new List<Nodo> ();
float angulo_coche = n_actual.angulo_hybrid;
float angulo_coche_rad = (Mathf.Deg2Rad * angulo_coche);
float x = 0.0f;
float z = 0.0f;
float y = 0.0f;
int indice_sucesor = 0;
//Calculamos la posicion continua
for (int i = 0; i < Constantes.numero_marchas; i++)
{
int direccion_giro = ((Constantes.numero_angulos - 1) / 2) * (-1);
for (int j = 0; j < Constantes.numero_angulos; j++)
{
Nodo sucesor = new Nodo();
float angulo_calculos = 0.0f;
x = n_actual.vector_hybrid.x;
z = n_actual.vector_hybrid.z;
y = n_actual.vector_hybrid.y;
sucesor.padre = n_actual;
if (i == 0) //hacia adelante
{
angulo_calculos = (radio_giro_rad * direccion_giro) + angulo_coche_rad;
sucesor.sentido = Constantes.hacia_adelante;
}
else //hacia atras
{
angulo_calculos = (radio_giro_rad * direccion_giro) + angulo_coche_rad + Constantes.angulo_atras_rad;
sucesor.sentido = Constantes.hacia_atras;
}
//Nuestro eje de referencia es Z
z = z + Constantes.vdt * Mathf.Cos(angulo_calculos);
x = x + Constantes.vdt * Mathf.Sin(angulo_calculos);
//Y sera la misma, si cambiase la altura seria y + altura de la posicion nueva, del suelo en el nuevo z,x
sucesor.vector_hybrid = new Vector3(x, y, z);
sucesor.angulo_hybrid = (radio_giro * direccion_giro) + n_actual.angulo_hybrid;
if (sucesor.angulo_hybrid < 0.0f)
{
sucesor.angulo_hybrid += 360.0f;
}
sucesores_array[indice_sucesor] = sucesor;
indice_sucesor++;
direccion_giro++;
}
}
//Calculamos la posicion discreta
//Calcular casilla
for (int i = 0; i < sucesores_array.Length; i++)
{
Nodo actual = sucesores_array [i];
float x_casilla_i = Mathf.Round(sucesores_array[i].vector_hybrid.x);
float y_casilla_i = Mathf.Round(sucesores_array[i].vector_hybrid.y);
float z_casilla_i = Mathf.Round(sucesores_array[i].vector_hybrid.z);
Vector3 nuevo_casilla = new Vector3(x_casilla_i, y_casilla_i, z_casilla_i);
actual.vector = nuevo_casilla;
sucesores_discretos.Add(actual);
//actual.vector = actual.vector_hybrid;
//sucesores_discretos.Add (actual);
}
//Calcular coste
foreach (Nodo sucesor_valido in sucesores_discretos)
{
sucesor_valido.costeG = funcionG(sucesor_valido);
sucesor_valido.costeH = funcionH(sucesor_valido, meta);
sucesor_valido.coste = (peso * sucesor_valido.costeG) + sucesor_valido.costeH;
}
//Descartar peores
for (int i = 0; i < sucesores_discretos.Count; i++)
{
int contador = 0;
for (int j = 1; j < sucesores_discretos.Count; j++)
{
if (sucesores_discretos [i].vector == sucesores_discretos [j].vector)
{
if (sucesores_discretos [i].coste > sucesores_discretos [j].coste)
{
contador++;
}
}
}
if (contador == 0)
{
sucesores_seleccionados.Add(sucesores_discretos [i]);
}
}
sucesores_validos = new Nodo[sucesores_seleccionados.Count];
for (int i = 0; i < sucesores_seleccionados.Count; i++)
{
sucesores_validos [i] = sucesores_seleccionados [i];
}
//Sucesores validos
sucesores = SucesoresValidos(sucesores_validos, mapa);
return(sucesores);
}
public abstract 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 dibujar_casillas);
// Para usar un algorutmo u otro
// Inicializacion
void Start()
{
int tam_parrilla;
GameObject suelo;
error = false;
encontrada_meta = false;
suelo = GameObject.FindGameObjectWithTag("Suelo");
largo = Mathf.FloorToInt((suelo.transform.localScale.z * 10.0f));
ancho = Mathf.FloorToInt((suelo.transform.localScale.x * 10.0f));
tam_parrilla = ancho * largo;
parrilla = new Parrilla(casilla_abiertos, casilla_cerrados, ancho, largo, z_casilla_distancias_0, z_casilla_distancias_1, z_casilla_distancias_2, z_casilla_distancias_3, z_casilla_distancias_4, z_casilla_distancias_5, z_casilla_distancias_6);
mapa = new ObtenerMapa();
coche = GameObject.FindGameObjectWithTag("Coche");
//rb_coche = coche.GetComponent<Rigidbody> ();
frontal = GameObject.FindGameObjectWithTag("Frontal");
trasera = GameObject.FindGameObjectWithTag("Trasera");
salida_trasera = trasera.transform.position;
salida_frontal = frontal.transform.position;
salida_coche = coche.transform.position;
// Elegir algoritmo
if (a_hybrid_a_estrella)
{
Debug.Log("Usando Hybrid A Estrella");
script_algoritmo = new Hybrid_a_estrella();
salida_coche = salida_frontal;
if (mapa.lineaVision(salida_coche, salida_frontal)) //no hace falta salir hacia atras
{
salida_coche = salida_frontal;
}
else if (mapa.lineaVision(salida_coche, salida_trasera)) //salir hacia atras
{
salida_coche = salida_trasera;
}
}
else if (a_A_estrella)
{
Debug.Log("Usando A Estrella");
script_algoritmo = new A_estrella();
}
else if (a_A_estrella_vertices)
{
Debug.Log("Usando A Estrella con vertices");
script_algoritmo = new A_estrella_vertices();
}
else if (a_theta_estrella)
{
Debug.Log("Usando Theta Estrella");
script_algoritmo = new Theta_estrella();
}
meta = GameObject.FindGameObjectWithTag("Meta").transform.position;
meta.y = meta.y - 0.01f; // Esto es porque esta posicionada elevada por motivos esteticos
angulo_coche = coche.transform.rotation.eulerAngles.y;
parrilla.Ancho = ancho;
parrilla.Largo = largo;
Debug.Log("Ancho: " + ancho + " | largo: " + largo);
encontrada_meta = false;
inicio = Time.realtimeSinceStartup;
script_algoritmo.iniciarCalcularRuta(salida_coche, meta, angulo_coche, mapa, parrilla, a_param_peso_heuristica, tam_parrilla, ancho, largo, a_dibujar_casillas);
if (a_dibujar_mapa_obstaculos)
{
dibujarMapaObstaculos(script_algoritmo.getMapaObstaculos());
}
else if (a_dibujar_mapa_distancias)
{
dibujarMapaDistancias(script_algoritmo.getMapaDistancias());
}
}
// A*
public override Vector3[] CalcularRuta(Vector3 inicio, Vector3 meta, float v_angulo_coche, ObtenerMapa mapa, Parrilla parrilla, float p_peso, int tam_parrilla, int ancho, int largo, bool v_dibujar_casillas)
{
bool error;
peso = p_peso;
iniciarCalcularRuta(inicio, meta, v_angulo_coche, mapa, parrilla, peso, tam_parrilla, ancho, largo, v_dibujar_casillas);
while (!pasoCalcularRuta(out error))
{
}
return(v_trayectoria);
}
public abstract Vector3[] CalcularRuta(Vector3 inicio, Vector3 meta, float v_angulo_coche, ObtenerMapa mapa, Parrilla parrilla, float p_peso, int tam_parrilla, int ancho, int largo, bool dibujar_casillas);
