private void btnSmooth_Click(object sender, EventArgs e)
{
btnSmooth.Enabled = false;
button1.Enabled = true;
timer1.Enabled = true;
/*
double[] p1 = new double[2]{0,0};
double[] p2 = new double[2]{0,1};
double[] p3 = new double[2]{1,1};
//double[] p4 = new double[2]{1,0};
Node node1 = new Node(p1);
Node node2 = new Node(p2);
node2.aCameFrom = node1;
Node node3 = new Node(p3);
node3.aCameFrom = node2;
List<Edge> edgeList = new List<Edge>();
edgeList.Add(new Edge(node1, node2, 1, EdgeState.Free));
edgeList.Add(new Edge(node2, node3, 1, EdgeState.Free));
*/
smoothing = new PathSmoothing(optmizer.bestDestNode, optmizer.CSpace);
/*
for (int i = 0; i < 100000; i++)
{
PathSmoothing.Smooth();
}
*/
//label1.Text = optmizer.bestDestNode.aTotalDist.ToString("0.00");
}
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);
}
/// <summary>
/// Makes the DStarLite calculate where the bot should move next and than moves the bot in that direction.
/// </summary>
private void NextMove(Vector2Int currentCoordinates)
{
// if the calculated distance between start and goal is infinite the goal is unreachable, generate new goal and return
if (float.IsInfinity((float)m_dStarLite.Map.GetNode(m_dStarLite.Start.x, m_dStarLite.Start.y).CostFromStartingPoint))
{
GenerateNewDestination(currentCoordinates);
return;
}
List <Vector2Int> coordinatesToTraverse = m_dStarLite.CoordinatesToTraverse();
Vector2Int farthestReachableNode = m_previousFarthestNode;
if (m_dStarLite.CoordinatesToTraverseChanged || currentCoordinates.Equals(m_previousFarthestNode)) // Perform pathsmoothing if either the path has changed, or if the bot is on the same position as the old farthest node.
{
farthestReachableNode = PathSmoothing.FarthestCoordinateToReach(new Vector2(gameObject.transform.position.x, gameObject.transform.position.z), coordinatesToTraverse, m_dStarLite.Map, OFFSET_FOR_LINE_CALCULATION);
}
m_previousFarthestNode = farthestReachableNode;
MoveTo(farthestReachableNode);
m_dStarLite.SyncBotPosition(currentCoordinates);
}
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 descensoGradienteTest()
{
Vector3[] trayectoria_prueba = new Vector3[9];
Vector3[] correcto = new Vector3[9];
Vector3[] resultado_prueba = new Vector3[9];
//ObtenerMapa mapa = new ObtenerMapa ();
trayectoria_prueba [0] = new Vector3(0.0f, 0.0f, 0.0f);
trayectoria_prueba [1] = new Vector3(0.0f, 0.0f, 1.0f);
trayectoria_prueba [2] = new Vector3(0.0f, 0.0f, 2.0f);
trayectoria_prueba [3] = new Vector3(1.0f, 0.0f, 2.0f);
trayectoria_prueba [4] = new Vector3(2.0f, 0.0f, 2.0f);
trayectoria_prueba [5] = new Vector3(3.0f, 0.0f, 2.0f);
trayectoria_prueba [6] = new Vector3(4.0f, 0.0f, 2.0f);
trayectoria_prueba [7] = new Vector3(4.0f, 0.0f, 3.0f);
trayectoria_prueba [8] = new Vector3(4.0f, 0.0f, 4.0f);
correcto [0] = new Vector3(0.0f, 0.0f, 0.0f);
correcto [1] = new Vector3(0.02941174f, 0.0f, 0.9705883f);
correcto [2] = new Vector3(0.1764706f, 0.0f, 1.823529f);
correcto [3] = new Vector3(1.029412f, 0.0f, 1.970588f);
correcto [4] = new Vector3(2.0f, 0.0f, 2.0f);
correcto [5] = new Vector3(2.970588f, 0.0f, 2.029412f);
correcto [6] = new Vector3(3.823529f, 0.0f, 2.176471f);
correcto [7] = new Vector3(3.970588f, 0.0f, 3.029412f);
correcto [8] = new Vector3(4.0f, 0.0f, 4.0f);
PathSmoothing pruebaPS = new PathSmoothing(null, trayectoria_prueba, 0.5f, 0.1f);
pruebaPS.setPesoTrayectoria(0.5f);
pruebaPS.setPesoSuavizado(0.1f);
pruebaPS.setTolerancia(0.000001f);
resultado_prueba = pruebaPS.descensoGradiente(trayectoria_prueba);
for (int i = 0; i < resultado_prueba.Length; i++)
{
Assert.IsTrue(resultado_prueba[i] == correcto[i], "NO son iguales: es " + resultado_prueba[i] + " | debia ser: " + correcto[i]);
}
}
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);
}
// Se ejecuta una vez cada frame
void Update()
{
bool fin;
// Solo lo usamos en caso de que se seleccione el control manual
input_fuerza = Input.GetAxis("Vertical");
input_angulo = Input.GetAxis("Horizontal");
if (error) // No se ha encontrado una ruta hasta la meta
{
Debug.Log("Error: no se ha encontrado un camino");
parrilla.borrarTodasCasillas();
}
else if (!encontrada_meta)
{
// Se ejecuta en cada update hasta que se encuentre la meta
fin = script_algoritmo.pasoCalcularRuta(out error);
if (fin) //Se ha encontrado la meta
{
final = Time.realtimeSinceStartup;
encontrada_meta = true;
Debug.Log("Ruta calculada en: " + (final - inicio));
parrilla.borrarTodasCasillas();
trayectoria = script_algoritmo.getTrayectoria();
trayectoria_nodos = script_algoritmo.getTrayectoriaNodos();
if (ps_path_smoothing_activado)
{
Vector3[] trayectoria_ps = trayectoria;
if (a_hybrid_a_estrella)
{
trayectoria_ps = trayectoriaHybrid(trayectoria_nodos, trayectoria_nodos.Length);
}
path_smoothing = new PathSmoothing(mapa, trayectoria_ps, ps_peso_trayectoria, ps_peso_suavizado);
if (ps_bezier)
{
trayectoria = path_smoothing.getTrayectoriaSuavizadaCurvasBezier();
}
else if (ps_descenso_gradiente)
{
trayectoria = path_smoothing.getTrayectoriaDescensoGradiente();
}
else
{
trayectoria = path_smoothing.eliminarZigZag(trayectoria);
}
}
contador_vector = 0;
if (a_hybrid_a_estrella)
{
DibujarTrayectoria(trayectoria_nodos, trayectoria_nodos.Length);
}
else
{
DibujarTrayectoria(trayectoria, trayectoria.Length);
}
pid = new PID_control(coche, trayectoria);
pid_hybrid = new PID_control_hybrid(coche, trayectoria_nodos);
pid.setParrilla(parrilla);
pid_hybrid.setParrilla(parrilla);
}
}
}
void Update()
{
if (Time.time > this.nextUpdateTime || this.GameManager.WorldChanged)
{
this.GameManager.WorldChanged = false;
if (this.MCTSDecisionMaking.BestActionSequence == null)
{
this.nextUpdateTime = Time.time;
}
else
{
this.nextUpdateTime = Time.time + DECISION_MAKING_INTERVAL;
}
if (this.MCTSDecisionMaking.BestActionSequence != null)
{
this.MCTSDecisionMaking.BestActionSequence.Clear();
}
//first step, perceptions
//update the agent's goals based on the state of the world
this.SurviveGoal.InsistenceValue = this.GameManager.characterData.MaxHP - this.GameManager.characterData.HP;
this.BeQuickGoal.InsistenceValue += DECISION_MAKING_INTERVAL * 0.1f;
if (this.BeQuickGoal.InsistenceValue > 10.0f)
{
this.BeQuickGoal.InsistenceValue = 10.0f;
}
this.GainXPGoal.InsistenceValue += this.GainXPGoal.ChangeRate; //increase in goal over time
if (this.GameManager.characterData.XP > this.previousXP)
{
this.GainXPGoal.InsistenceValue -= (this.GameManager.characterData.XP - this.previousXP) / 2;
this.previousXP = this.GameManager.characterData.XP;
}
this.GetRichGoal.InsistenceValue += this.GetRichGoal.ChangeRate; //increase in goal over time
if (this.GetRichGoal.InsistenceValue > 10)
{
this.GetRichGoal.InsistenceValue = 10.0f;
}
if (this.GameManager.characterData.Money > this.previousGold)
{
this.GetRichGoal.InsistenceValue -= this.GameManager.characterData.Money - this.previousGold;
this.previousGold = this.GameManager.characterData.Money;
}
this.SurviveGoalText.text = "Survive: " + this.SurviveGoal.InsistenceValue;
this.GainXPGoalText.text = "Gain XP: " + this.GainXPGoal.InsistenceValue.ToString("F1");
this.BeQuickGoalText.text = "Be Quick: " + this.BeQuickGoal.InsistenceValue.ToString("F1");
this.GetRichGoalText.text = "GetRich: " + this.GetRichGoal.InsistenceValue.ToString("F1");
//initialize Decision Making Proccess
this.CurrentAction = null;
if (this.MCTSActive)
{
this.MCTSDecisionMaking.InitializeMCTSearch();
}
else
{
this.GOAPDecisionMaking.InitializeDecisionMakingProcess();
}
}
if (this.MCTSActive)
{
this.UpdateMCTS();
}
else
{
this.UpdateDLGOAP();
}
if (this.CurrentAction != null)
{
if (this.CurrentAction.CanExecute())
{
this.CurrentAction.Execute();
}
}
//call the pathfinding method if the user specified a new goal
if (this.AStarPathFinding.InProgress)
{
var finished = this.AStarPathFinding.Search(out this.currentSolution);
if (finished && this.currentSolution != null)
{
//lets smooth out the Path
this.startPosition = this.Character.KinematicData.position;
//this.currentSmoothedSolution = StringPullingPathSmoothing.SmoothPath(this.Character.KinematicData.position, this.currentSolution);
this.currentSmoothedSolution = PathSmoothing.SmoothPath(this.startPosition, this.currentSolution.PathPositions.Last(), this.currentSolution, navMesh);
//this.currentSmoothedSolution = this.currentSolution;
this.currentSmoothedSolution.CalculateLocalPathsFromPathPositions(this.Character.KinematicData.position);
this.Character.Movement = new DynamicFollowPath(this.Character.KinematicData, this.currentSmoothedSolution)
{
MaxAcceleration = 200.0f,
MaxSpeed = 40.0f
};
}
}
this.Character.Update();
//manage the character's animation
if (this.Character.KinematicData.velocity.sqrMagnitude > 0.1)
{
this.characterAnimator.SetBool("Walking", true);
}
else
{
this.characterAnimator.SetBool("Walking", false);
}
}
void Awake()
{
instance = this;
pathfinding = GetComponent<Pathfinding>();
pathSmoothing = GetComponent<PathSmoothing>();
}
