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>(); }