public string Proximas_Secuencias_de_movimiento(ListBox lb_distancias, ListBox lb_iteraciones)
{
if (this.optimal_path != null && this.optimal_path.Count != 0 && this.index_path != -1)
{
List <TRNode> temp_path = this.optimal_path[index_path++];
temp_path.Insert(0, new TRNode(this.arm[this.arm.Count - 1]));
List <TRNode> current_path = TREXT_TrajectoryGraph.GetThinPartition(temp_path, this.config.TR_Separacion_entre_nodos);
Publica_lo_basico();
this.progres_bar.Visible = true;
this.progres_bar.Maximum = current_path.Count;
ParticleFilter pf = new ParticleFilter();
List <TMPoint> chain_result = null;
int iteraciones = 0;
double distancia = 0;
this.progres_bar.Minimum = 0;
#region codigo para verificar en matlab
//string s_matlab = "[" + current_path[0].Point.X.ToString() + "," + current_path[0].Point.Y.ToString();
//for (int i = 1; i < current_path.Count; i++)
//{
// string x_pe = Math.Floor(current_path[i].Point.X).ToString();
// string x_pf = (current_path[i].Point.X - Math.Floor(current_path[i].Point.X)).ToString();
// if(x_pf != "0")
// x_pf = x_pf.Substring(2);
// string sx = x_pe + "." + x_pf;
// string y_pe = Math.Floor(current_path[i].Point.Y).ToString();
// string y_pf = (current_path[i].Point.Y - Math.Floor(current_path[i].Point.Y)).ToString();
// if (y_pf != "0")
// y_pf = y_pf.Substring(2);
// string sy = y_pe + "." + y_pf;
// s_matlab += "; " + sx + "," + sy;
//}
//s_matlab += "]";
#endregion
List <TMPoint> temp_arm = this.arm;
//for (int i = index_firts_node; i < current_path.Count; i++)
for (int i = 0; i < current_path.Count; i++)
{
this.progres_bar.Increment(1);
bool llego = pf.Filter(out chain_result, out iteraciones, out distancia, config.OPT_Maximo_iteraciones, config.PF_Epsilon, this.sistema_obstaculos, temp_arm, current_path[i].Point, config.PF_Numero_muestras, config.PF_Media_Gausiana);
Publicar_Brazo(chain_result);
lb_distancias.Items.Add(distancia);
string sllega = (llego) ? "SI" : "NO";
lb_iteraciones.Items.Add(iteraciones.ToString() + " - " + sllega);
temp_arm = chain_result;
}
if (index_path >= optimal_path.Count)
{
this.index_path = -1;
}
this.progres_bar.Visible = false;
return("OK");
}
else
{
return("Se han terminado los caminos posibles a seguir");
}
}
public string Proximas_Secuencias_de_movimiento(ListBox lb_distancias, ListBox lb_iteraciones)
{
if (this.optimal_path != null && this.optimal_path.Count != 0 && this.index_path != -1)
{
List <TRNode> temp_path = this.optimal_path[index_path++];
temp_path.Insert(0, new TRNode(this.arm[this.arm.Count - 1]));
List <TRNode> current_path = TREXT_TrajectoryGraph.GetThinPartition(temp_path, this.config.TR_Separacion_entre_nodos);
Publica_lo_basico();
this.progres_bar.Visible = true;
this.progres_bar.Maximum = current_path.Count;
pf = new ParticleFilter();
this.progres_bar.Minimum = 0;
#region codigo para verificar en matlab
//string s_matlab = "[" + current_path[0].Point.X.ToString() + "," + current_path[0].Point.Y.ToString();
//for (int i = 1; i < current_path.Count; i++)
//{
// string x_pe = Math.Floor(current_path[i].Point.X).ToString();
// string x_pf = (current_path[i].Point.X - Math.Floor(current_path[i].Point.X)).ToString();
// if(x_pf != "0")
// x_pf = x_pf.Substring(2);
// string sx = x_pe + "." + x_pf;
// string y_pe = Math.Floor(current_path[i].Point.Y).ToString();
// string y_pf = (current_path[i].Point.Y - Math.Floor(current_path[i].Point.Y)).ToString();
// if (y_pf != "0")
// y_pf = y_pf.Substring(2);
// string sy = y_pe + "." + y_pf;
// s_matlab += "; " + sx + "," + sy;
//}
//s_matlab += "]";
#endregion
List <TMPoint> temp_arm = this.arm;
//for (int i = index_firts_node; i < current_path.Count; i++)
const int Top_MaxIter = 50;
double Top_esp = Math.Pow(10, -8);
int cant_iteraciones = 0;
double suma_distancia = 0;
double distancia = 0;
foreach (TRNode t in current_path)
{
int indexOf_t = current_path.IndexOf(t);
this.progres_bar.Increment(1);
List <TMPoint> chain_result;
int iteraciones;
double w;
bool llego =
pf.MATLAB_Filter(out chain_result,
out iteraciones,
out distancia,
out w,
indexOf_t == current_path.Count - 1 ? Top_MaxIter : config.PF_Maximo_iteraciones,
indexOf_t == current_path.Count - 1 ? Top_esp : config.PF_Epsilon,
this.sistema_obstaculos,
temp_arm,
t.Point,
config.PF_Numero_muestras,
config.PF_Media_Gausiana);
//pf.CS_Filter( out chain_result, out iteraciones, out distancia, out w, config.OPT_Maximo_iteraciones, config.PF_Epsilon, this.sistema_obstaculos, temp_arm, t.Point, config.PF_Numero_muestras, config.PF_Media_Gausiana );
Publicar_Brazo(chain_result);
if (w == 0)
{
MessageBox.Show(string.Format("Mejor partícula con peso 0"));
}
lb_distancias.Items.Add(distancia);
string sllega = (llego) ? "SI" : "NO";
lb_iteraciones.Items.Add(string.Format("{0} - {1}", iteraciones, sllega));
temp_arm = chain_result;
suma_distancia += distancia;
cant_iteraciones++;
}
if (index_path >= optimal_path.Count)
{
this.index_path = -1;
}
this.progres_bar.Visible = false;
return(string.Format("Separación promedio de la trayectoria {0} px\nSeparación final del e.e. al objetivo {1}", suma_distancia / cant_iteraciones, distancia));
}
return("Se han terminado los caminos posibles a seguir");
}
