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]);
}
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);
}
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 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 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 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 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);
}
// 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 void setParrilla(Parrilla p_parrilla)
{
parrilla = p_parrilla;
parrilla.crearCasilla(trayectoria[punto_actual].vector_hybrid, 0);
}
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);
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);
