public bool PointInside(TMPoint p)
{
bool x_rang = p.X >= this.LeftP.X && p.X < this.RightP.X;
bool y_rang = p.Y <= this.Top.eval(p.X) && p.Y >= this.Bottom.eval(p.Y);
return(x_rang && y_rang);
}
private static List <TRNode> aux_GetThinPartition(List <TRNode> path, double eps)
{
List <TRNode> ext = null;
if (path != null && path.Count > 0)
{
ext = new List <TRNode>();
ext.Add(path[0]);
TRNode node_actual = path[0];
for (int i = 1; i < path.Count; i++)
{
TRPuente puente = new TRPuente(node_actual.Point, path[i].Point);
double dx = delta_x(puente, eps);
double dy = delta_y(puente, eps);
while (norma(node_actual.Point, path[i].Point) > eps)
{
TMPoint point_actual = new TMPoint(node_actual.Point.X + dx, node_actual.Point.Y + dy);
node_actual = new TRNode(point_actual);
ext.Add(node_actual);
}
ext.Add(new TRNode(path[i].Point));
node_actual = path[i];
}
}
return(ext);
}
/// <summary>
///
/// </summary>
/// <param name="D"></param>
/// <param name="end_efector"></param>
/// <param name="objetivo"></param>
/// <param name="seps">rango que se tomará en cuenta para no acercar el nodo al obstáculo</param>
public TRTrajectoryGraph(TMSearchGraph D, TMPoint end_efector, TMPoint objetivo, double seps)
{
this.stack = new Stack <TRNode>();
this.current = null;
this.separacion = seps;
initGraph(D, end_efector, objetivo);
}
protected void initGraph(TMSearchGraph D, TMPoint end_efector, TMPoint objetivo)
{
var tr_inicio = (TMANode)D.Root.buscar(end_efector);
var tr_fin = (TMANode)D.Root.buscar(objetivo);
init_recursivo(null, D, tr_inicio, tr_fin, objetivo);
}
public override TMNode buscar(TMPoint p)
{
if (seg.porDebajoDe(p))
{
return(this.node_left.buscar(p));
}
return(this.node_rigth.buscar(p));
}
public override TMNode buscar(TMPoint p, TMSegment s)
{
if (p.alaDerechaDe(this.point) || p.Equals(point))
{
return(this.node_rigth.buscar(p, s));
}
return(this.node_left.buscar(p, s));
}
public TRNode(TMPoint p, TRNode u, TRNode l, TRRange r)
{
this.Lower = l;
this.Upper = u;
this.Point = p;
this.state = Node_State.Free;
this.Y_Range = r;
this.IsObjetive = false;
}
public TRNode(TMPoint p, TRNode u, TRNode l, TRRange r)
{
this.lower = l;
this.upper = u;
this.point = p;
this.state = Node_State.Free;
this.rango = r;
this.obj = false;
}
public TMTrapezoid(TMPoint leftp, TMPoint rightp, TMPoint top_left, TMPoint top_right, TMPoint bottom_left, TMPoint bottom_right)
{
this.LeftP = leftp;
this.RightP = rightp;
this.Top_Left = top_left;
this.Top_Right = top_right;
this.Bottom_Left = bottom_left;
this.Bottom_Right = bottom_right;
}
private void descubre_trapecio(GraphPane pane, params TMTrapezoid[] L)
{
LineItem curve;
for (int i = 0; i < L.Length; i++)
{
TMTrapezoid T = L[i];
TMPoint p = T.Centro;
curve = pane.AddCurve(null, new double[] { p.X }, new double[] { p.Y }, Color.Blue, SymbolType.Circle);
}
}
/// <summary>
/// Los ejemplos del 1 al 4 son no alcanzables
/// Del 5 al 7 son alcanzables. Los poligonos se construyen comenzando por el vertice que menos coordenada
/// "Y" tenga (menor "X" en caso de empate) y siguiendo el orden inverso de las manesillas del reloj
/// </summary>
private void cargar_ejemplos()
{
this.sistema_obstaculos = new TMObstacleSystem(this.obstaculos);
C_Free = this.sistema_obstaculos.freeConfigurationSpace(universe, out this.grafo_busqueda);
//GraphPane pane = zgc.GraphPane;
zgc.AxisChange();
relena_Arbol(tv, this.grafo_busqueda.Root);
var end_efector = new TMPoint(5, 40);
grafo_trayectorias = new TREXT_TrajectoryGraph(this.grafo_busqueda, end_efector, this.objetivo, config.TR_Separacion_Obstaculos);
optimal_path = new List <List <TRNode> >();
optimizar_camino();
this.index_path = 0;
Publica_lo_basico();
}
public override TMNode buscar(TMPoint p, TMSegment s)
{
if (seg.porDebajoDe(p))
{
return(this.node_left.buscar(p, s));
}
if (seg.porEncimaDe(p))
{
return(this.node_rigth.buscar(p, s));
}
if (s.Pendiente < seg.Pendiente)
{
return(this.node_rigth.buscar(p, s));
}
return(this.node_left.buscar(p, s));
}
private TMPoligonList ejemplo7(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
TMPoligon S1 = new TMPoligon(1, new double[] { 40, 100, 90, 50 }, new double[] { 40, 40, 60, 60 });
List <TMPoligon> l = new List <TMPoligon>();
l.Add(S1);
obj = new TMPoint(70, 67);
double[] x_chain = new double[] { 0, 10, 43 };
double[] y_chain = new double[] { 0, 60, 30 };
arm = new List <TMPoint>();
arm.Add(new TMPoint(x_chain[0], y_chain[0]));
arm.Add(new TMPoint(x_chain[1], y_chain[1]));
arm.Add(new TMPoint(x_chain[2], y_chain[2]));
return(new TMPoligonList(l));
}
private TMPoligonList ejemplo3(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
TMPoligon S1 = new TMPoligon(1, new double[] { 50, 80, 70, 40 }, new double[] { 40, 50, 70, 60 });
TMPoligon S2 = new TMPoligon(2, new double[] { 85, 120, 110 }, new double[] { 47, 50, 70 });
TMPoligon S3 = new TMPoligon(3, new double[] { 60, 90, 100, 65 }, new double[] { 20, 20, 40, 40 });
List <TMPoligon> l = new List <TMPoligon>();
l.Add(S1); l.Add(S2); l.Add(S3);
obj = new TMPoint(80, 45);
double[] x_chain = new double[] { 0, 5, 20 };
double[] y_chain = new double[] { 0, 50, 20 };
arm = new List <TMPoint>();
arm.Add(new TMPoint(x_chain[0], y_chain[0]));
arm.Add(new TMPoint(x_chain[1], y_chain[1]));
arm.Add(new TMPoint(x_chain[2], y_chain[2]));
return(new TMPoligonList(l));
}
private TMPoligonList ejemplo6(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
TMPoligon S1 = new TMPoligon(1, new double[] { 40, 50, 30 }, new double[] { 30, 50, 50 });
TMPoligon S2 = new TMPoligon(2, new double[] { 90, 110, 80 }, new double[] { 30, 30, 60 });
List <TMPoligon> l = new List <TMPoligon>();
l.Add(S1);
l.Add(S2);
obj = new TMPoint(70, 50);
double[] x_chain = new double[] { 0, 10, 15, 20 };
double[] y_chain = new double[] { 0, 30, 6, 55 };
arm = new List <TMPoint>();
arm.Add(new TMPoint(x_chain[0], y_chain[0]));
arm.Add(new TMPoint(x_chain[1], y_chain[1]));
arm.Add(new TMPoint(x_chain[2], y_chain[2]));
arm.Add(new TMPoint(x_chain[3], y_chain[3]));
return(new TMPoligonList(l));
}
public bool CS_Filter
(
out List <TMPoint> chain_result,
out int it,
out double distance,
out double w_best_particle,
int max_iter,
double epsilon,
TMObstacleSystem S,
List <TMPoint> arm,
TMPoint obj,
int samples,
double m_gaussian
)
{
var obstaculos = new List <Obstaculo>();
foreach (TMPoligon poligon in S.Obstacles.Poligons)
{
var xVector = new double[poligon.Points.Count];
var yVector = new double[poligon.Points.Count];
for (int i = 0; i < poligon.Points.Count; i++)
{
xVector[i] = poligon.Points[i].X;
yVector[i] = poligon.Points[i].Y;
}
obstaculos.Add(new Obstaculo(xVector, yVector));
}
var arm_ = arm.Select(tmPoint => new PointD(tmPoint.X, tmPoint.Y)).ToList();
var particleFilter = new CS_ParticleFilter.ParticleFilter();
FilterResult filterResult = particleFilter.PrtFltr_MkwskSum(max_iter, epsilon,
new SistemaObstaculos(obstaculos),
arm_,
new PointD(obj.X, obj.Y),
samples,
m_gaussian);
chain_result = filterResult.ChainResult_points.Select(point => new TMPoint(point.X, point.Y)).ToList();
it = filterResult.Iterations;
distance = filterResult.Distance;
w_best_particle = filterResult.ChainResult.W;
return(distance <= epsilon);
}
private TMPoligonList ejemplo7(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
var S1 = new TMPoligon(1, new double[] { 40, 100, 90, 50 }, new double[] { 40, 40, 60, 60 });
var l = new List <TMPoligon> {
S1
};
obj = new TMPoint(70, 67);
var x_chain = new double[] { 0, 10, 43 };
var y_chain = new double[] { 0, 60, 30 };
arm = new List <TMPoint>
{
new TMPoint(x_chain[0], y_chain[0]),
new TMPoint(x_chain[1], y_chain[1]),
new TMPoint(x_chain[2], y_chain[2])
};
return(new TMPoligonList(l));
}
private TMPoligonList ejemplo4(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
var S1 = new TMPoligon(1, new double[] { 33, 63, 60, 30 }, new double[] { 17, 60, 63, 20 });
var l = new List <TMPoligon> {
S1
};
obj = new TMPoint(70, 55);
var x_chain = new double[] { 0, 10, 25 };
var y_chain = new double[] { 0, 50, 15 };
arm = new List <TMPoint>
{
new TMPoint(x_chain[0], y_chain[0]),
new TMPoint(x_chain[1], y_chain[1]),
new TMPoint(x_chain[2], y_chain[2])
};
return(new TMPoligonList(l));
}
private TMPoligonList ejemplo1(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
var S1 = new TMPoligon(1, new double[] { 40, 110, 100, 50 }, new double[] { 40, 40, 60, 60 });
var S2 = new TMPoligon(2, new double[] { 75, 90, 60 }, new double[] { 20, 30, 30 });
var l = new List <TMPoligon> {
S1, S2
};
obj = new TMPoint(100, 35);
var x_chain = new double[] { 0, 5, 20 };
var y_chain = new double[] { 0, 50, 20 };
arm = new List <TMPoint>
{
new TMPoint(x_chain[0], y_chain[0]),
new TMPoint(x_chain[1], y_chain[1]),
new TMPoint(x_chain[2], y_chain[2])
};
return(new TMPoligonList(l));
}
private TMPoligonList ejemplo3(out TMPoligon universe, out TMPoint obj)
{
universe = new TMPoligon(0, new double[] { 0, 140, 140, 0 }, new double[] { 0, 0, 80, 80 });
var S1 = new TMPoligon(1, new double[] { 50, 80, 70, 40 }, new double[] { 40, 50, 70, 60 });
var S2 = new TMPoligon(2, new double[] { 85, 120, 110 }, new double[] { 47, 50, 70 });
var S3 = new TMPoligon(3, new double[] { 60, 90, 100, 65 }, new double[] { 20, 20, 40, 40 });
var l = new List <TMPoligon> {
S1, S2, S3
};
obj = new TMPoint(80, 45);
var x_chain = new double[] { 0, 5, 20 };
var y_chain = new double[] { 0, 50, 20 };
arm = new List <TMPoint>
{
new TMPoint(x_chain[0], y_chain[0]),
new TMPoint(x_chain[1], y_chain[1]),
new TMPoint(x_chain[2], y_chain[2])
};
return(new TMPoligonList(l));
}
public TMPNode(TMPoint p)
{
this.point = p;
}
internal TRPuente(TMPoint p1, TMPoint p2)
{
this.p1 = p1;
this.p2 = p2;
}
private static double norma(TMPoint p1, TMPoint p2)
{
double n = Math.Sqrt((p2.X - p1.X) * (p2.X - p1.X) + (p2.Y - p1.Y) * (p2.Y - p1.Y));
return(n);
}
public TREXT_TrajectoryGraph(TMSearchGraph D, TMPoint end_efector, TMPoint objetivo, double seps)
: base(D, end_efector, objetivo, seps)
{
}
public TRNode(TMPoint p, TRRange r)
: this(p, null, null, r)
{
}
public void ejemplo_trapecios(ZedGraphControl zgc)
{
// P1 = ejemplo_punto(10, 30)
TMPoint P1 = new TMPoint(10, 30);
// A = ejemplo_trapecio(0, 60, 10, 60, 0, 0, 10, 0, 0, 0, 10, 30);
TMTrapezoid A = new TMTrapezoid(new TMPoint(0, 0), new TMPoint(10, 30),
new TMPoint(0, 60), new TMPoint(10, 60), new TMPoint(0, 0), new TMPoint(10, 0));
// Q1 = ejemplo_punto(40, 50);
TMPoint Q1 = new TMPoint(40, 50);
// S1 = ejemplo_segmento(P1, Q1);
TMSegment S1 = new TMSegment(P1, Q1);
// B = ejemplo_trapecio(10, 60, 40, 60, 10, 30, 40, 50, 10, 30, 40, 50);
TMTrapezoid B = new TMTrapezoid(new TMPoint(10, 30), new TMPoint(40, 50),
new TMPoint(10, 60), new TMPoint(40, 60), new TMPoint(10, 30), new TMPoint(40, 50));
// P2 = ejemplo_punto(20, 20);
TMPoint P2 = new TMPoint(20, 20);
// C = ejemplo_trapecio(10, 30, 20, evalua_recta(S1, 20), 10, 0, 20, 0, P1.x, P1.y, P2.x, P2.y);
TMTrapezoid C = new TMTrapezoid(P1, P2,
new TMPoint(10, 30), new TMPoint(20, S1.eval(20)), new TMPoint(10, 0), new TMPoint(20, 0));
// Q2 = ejemplo_punto(50, 10);
TMPoint Q2 = new TMPoint(50, 10);
// S2 = ejemplo_segmento(P2, Q2);
TMSegment S2 = new TMSegment(P2, Q2);
// D = ejemplo_trapecio(20, evalua_recta(S1, 20), 40, 50, 20, 20, 40, evalua_recta(S21, 40), P2.x, P2.y, Q1.x, Q1.y);
TMTrapezoid D = new TMTrapezoid(P2, Q1,
new TMPoint(20, S1.eval(20)), new TMPoint(40, 50), new TMPoint(20, 20), new TMPoint(40, S2.eval(40)));
// F = ejemplo_trapecio(P2.x, P2.y, Q2.x, Q2.y, 20, 0, 50, 0, P2.x, P2.y, Q2.x, Q2.y);
TMTrapezoid F = new TMTrapezoid(P2, Q2, P2, Q2, new TMPoint(20, 0), new TMPoint(50, 0));
// E = ejemplo_trapecio(40, 60, 50, 60, 40, evalua_recta(S21, 40), Q2.x, Q2.y, Q1.x, Q1.y, Q2.x, Q2.y);
TMTrapezoid E = new TMTrapezoid(Q1, Q2, new TMPoint(40, 60), new TMPoint(50, 60), new TMPoint(40, S2.eval(40)), Q2);
// G = ejemplo_trapecio(50, 60, 60, 60, 50, 0, 60, 0, Q2.x, Q2.y, 60, 60);
TMTrapezoid G = new TMTrapezoid(Q2, new TMPoint(60, 60),
new TMPoint(50, 60), new TMPoint(60, 60), new TMPoint(50, 0), new TMPoint(60, 0));
// Enlaces
//TMPNode nP1 = new TMPNode(P1);
//TMANode nA = new TMANode(A);
//TMPNode nQ1 = new TMPNode(Q1);
//TMSNode nS1 = new TMSNode(S1);
//TMANode nB = new TMANode(B);
//TMPNode nP2 = new TMPNode(P2);
//TMANode nC = new TMANode(C);
//TMSNode nS21 = new TMSNode(S2);
//TMANode nD = new TMANode(D);
//TMANode nF = new TMANode(F);
//TMPNode nQ2 = new TMPNode(Q2);
//TMANode nG = new TMANode(G);
//TMSNode nS22 = new TMSNode(S2);
//TMANode nE = new TMANode(E);
//nS21.Node_Left = nD; //S21.node_izq = D;
//nS21.Node_Rigth = nF; //S21.node_der = F;
//nS22.Node_Left = nE; //S22.node_izq = E;
//nS22.Node_Rigth = nF; //S22.node_der = F;
//nQ2.Node_Left = nS22; //Q2.node_izq = S22;
//nQ2.Node_Rigth = nG; //Q2.node_der = G;
//nP2.Node_Left = nC; //P2.node_izq = C;
//nP2.Node_Rigth = nS21; //P2.node_der = S21;
//nS1.Node_Left = nB; //S1.node_izq = B;
//nS1.Node_Rigth = nP2; //S1.node_der = P2;
//nQ1.Node_Left = nS1; //Q1.node_izq = S1;
//nQ1.Node_Rigth = nQ2; //Q1.node_der = Q2;
//nP1.Node_Left = nA; //P1.node_izq = A;
//nP1.Node_Rigth = nQ1; //P1.node_der = Q1;
//GraphPane pane = zgc.GraphPane;
//pane.Title.Text = "Ejemplo del libro";
//pane.XAxis.Title.Text = "X Axis";
//pane.YAxis.Title.Text = "Y Axis";
//plotTrapezoid(pane, A, B, C, D, E, F, G);
//plotSegment(pane, S1, S2);
//plotdot(pane, P1, P2, Q1, Q2);
//TMTrapezoid R = new TMTrapezoid(new TMPoint(0, 0), new TMPoint(100, 100),
// new TMPoint(0, 100), new TMPoint(100, 100),
// new TMPoint(0, 0), new TMPoint(100, 0));
//TMANode R_ = new TMANode(R);
//List<TMNode> T = new List<TMNode>();
//T.Add(R_);
//TMPoint ps1 = new TMPoint(15, 25);
//TMPoint ps2 = new TMPoint(45, 35);
//TMSegment SS = new TMSegment(ps1, ps2);
//TMSearchGraph DD = new TMSearchGraph(nP1);
//T = DD.follow_segment(SS);
//descubre_trapecio(pane, T.ToArray());
//zgc.AxisChange();
}
protected void init_recursivo(TRNode anterior, TMSearchGraph D, TMANode tr, TMANode tr_fin, TMPoint objetivo)
{
TMPoint centro = tr.Trap.Centro;
TRNode node_centro = (anterior == null) ?
new TRNode(centro, new TRRange(centro.Y, centro.Y))
: new TRNode(centro, rango_del_centro(tr.Trap, centro.Y));
if (anterior != null)
{
if (anterior.Upper == null)
{
anterior.Upper = node_centro;
}
else
{
anterior.Lower = node_centro;
}
}
else
{
this.root = node_centro;
}
if (tr.Equals(tr_fin))
{
//TRNode node_objetivo = new TRNode(objetivo, new TRRange(objetivo.Y, objetivo.Y));
//node_objetivo.IsObjetive = true;
//node_centro.Upper = node_objetivo;
var node_objetivo = new TRNode(objetivo, new TRRange(objetivo.Y, objetivo.Y))
{
IsObjetive = true
};
if (anterior.Upper.Equals(node_centro))
{
anterior.Upper = node_objetivo;
}
else
{
anterior.Lower = node_objetivo;
}
}
else
{
TMANode lower_right = D.lower_right(tr);
TMANode upper_right = D.upper_right(tr);
// Se verifica que alguno de ellos no es null
// Y
// [
// Upper_Right no es null y si el otro no es null entonces no son iguales
// O
// Son iguales y el top del trapecio entonces es mayor que el rightp
// ]
bool se_construye_el_upper = (upper_right != null || lower_right != null)
&&
(((upper_right != null) && (lower_right == null || !lower_right.Equals(upper_right)))
||
(lower_right.Equals(upper_right) && tr.Trap.RightP.Y < tr.Trap.Top_Right.Y));
bool se_construye_el_lower = (upper_right != null || lower_right != null)
&&
(((lower_right != null) && (upper_right == null || !lower_right.Equals(upper_right)))
||
(upper_right.Equals(lower_right) && tr.Trap.RightP.Y > tr.Trap.Bottom_Right.Y));
if (se_construye_el_upper)
{
double xpu = tr.Trap.RightP.X;
double ypu = (tr.Trap.RightP.Y + tr.Trap.Top_Right.Y) / 2;
var pu = new TMPoint(xpu, ypu);
var node_upper = new TRNode(pu, rango_derecho(tr.Trap, ypu));
node_centro.Upper = node_upper;
init_recursivo(node_upper, D, upper_right, tr_fin, objetivo);
}
if (se_construye_el_lower)
{
double xpw = tr.Trap.RightP.X;
double ypw = (tr.Trap.RightP.Y + tr.Trap.Bottom_Right.Y) / 2;
var pw = new TMPoint(xpw, ypw);
var node_lower = new TRNode(pw, rango_derecho(tr.Trap, ypw));
node_centro.Lower = node_lower;
init_recursivo(node_lower, D, lower_right, tr_fin, objetivo);
}
if (!se_construye_el_upper && !se_construye_el_lower)
{
node_centro.Y_Range = new TRRange(centro.Y, centro.Y);
}
}
}
/**
* % Esta funcion es la que engloba los algoritmos de Filtro de Particulas
* % y Suma de Minkowski. Como parámetros se le pasa:
* % max_iter: Maximo de iteraciones que realizara el filtraje
* % epsilon: tolerancia de cercanía con el objetivo
* % S: el sistema de obstaculos
* % x_axis: posiciones en el eje x de la cadena en su posición "inicial"
* % y_axis: posiciones en el eje y de la cadena en su posición "inicial"
* % obj: objetivo intermedio
* % samples: cantidad de particulas que se generaran
* % media_gausiana: media para la generación de particulas (0.03)
* % S:
* % S = [O1 O2 ... Ok]
* % Ok.ptos: lista de puntos, ordenados tomando como principio el que menos
* % coordenada "y" tenga (menos "x") en caso de empate y siguiendo el orden
* % inverso a a las manesillas del reloj
**/
/// <summary>
/// Este metodo se encarga de encontrar la configuracion que debe adoptar el brazo (arm) para llegar al objetivo (obj)
/// </summary>
/// <param name="chain_result">(Salida) Posiciones de la cadena resultante</param>
/// <param name="it">(Salida) Numero de iteraciones realizadas</param>
/// <param name="distance">(Salida) Distancia a la que quedo del objetivo intermedio (obj)</param>
/// <param name="w_best_particle"></param>
/// <param name="max_iter">Maximo de iteraciones que realizara el filtro</param>
/// <param name="epsilon">Distancia a la que se quiere llegar del objetivo intermedio (obj)</param>
/// <param name="S">Sistemas de obstaculos</param>
/// <param name="arm">Posiciones de los nodos de la cadena inicialmente</param>
/// <param name="obj">Posicion del objetivo intermedio</param>
/// <param name="samples">Numero de particulas que se generaran en el proceso de filtrado</param>
/// <param name="m_gaussian">Media gaussiana</param>
/// <returns>Indica si el brazo pudo llegar al objetivo.</returns>
public bool MATLAB_Filter
(
out List <TMPoint> chain_result,
out int it,
out double distance,
out double w_best_particle,
int max_iter,
double epsilon,
TMObstacleSystem S,
List <TMPoint> arm,
TMPoint obj,
int samples,
double m_gaussian
)
{
//chain_result = null;
//it = 0;
//distance = 0;
//PF = new MATLAB_ParticleFilter.MatlabParticleFilter_v2();
//function [chainResult, chainResult_points, it, distance] = PrtFltr_MkwskSum(max_iter, epsilon, S, x_axis, y_axis, obj, samples, media_gausiana)
MWArray MAT_max_iter = new MWNumericArray(max_iter);
MWArray MAT_epsilon = new MWNumericArray(epsilon);
#region OBSTACULOS
if (this.MAT_obst_ranks == null || this.MAT_x_obst == null || this.MAT_y_obst == null)
{
int obs_count = S.Obstacles.Poligons.Count;
/*
* obst_ranks = [4];
* x_obst = [0.4 0.7 0.7 0.4];
* y_obst = [0.6 0.6 0.9 0.9];
*/
var rr = new int[obs_count];
int c_ptos = 0;
for (int i = 0; i < obs_count; i++)
{
c_ptos += S.Obstacles.Poligons[i].Count;
rr[i] = c_ptos;
}
this.MAT_obst_ranks = new MWNumericArray(1, obs_count, rr);
var x_rr = new double[c_ptos];
var y_rr = new double[c_ptos];
int index = 0;
foreach (TMPoligon pol in S.Obstacles.Poligons)
{
foreach (TMPoint p in pol.Points)
{
x_rr[index] = p.X;
y_rr[index] = p.Y;
index++;
}
}
//corrimiento_minkowski(rr, out x_rr, out y_rr);
this.MAT_x_obst = new MWNumericArray(1, c_ptos, x_rr);
this.MAT_y_obst = new MWNumericArray(1, c_ptos, y_rr);
}
#endregion
var a_x = new double[arm.Count];
var a_y = new double[arm.Count];
for (int i = 0; i < arm.Count; i++)
{
a_x[i] = arm[i].X;
a_y[i] = arm[i].Y;
}
MWArray MAT_X_axis = new MWNumericArray(1, arm.Count, a_x);
MWArray MAT_Y_axis = new MWNumericArray(1, arm.Count, a_y);
MWArray MAT_Obj = new MWNumericArray(1, 2, new[] { obj.X, obj.Y });
MWArray MAT_Samples = new MWNumericArray(samples);
MWArray MAT_MedGauss = new MWNumericArray(m_gaussian);
//MWArray t = new MWStructArray();
MWArray[] outs = PF.PrtFltr_MkwskSum(4, MAT_max_iter, MAT_epsilon, MAT_obst_ranks, MAT_x_obst, MAT_y_obst, MAT_X_axis, MAT_Y_axis, MAT_Obj, MAT_Samples, MAT_MedGauss);
w_best_particle = ((MWNumericArray)((MWStructArray)outs[0])["w"]).ToScalarDouble();
distance = ((MWNumericArray)outs[3]).ToScalarDouble();
it = ((MWNumericArray)outs[2]).ToScalarInteger();
int ptos_brazos = outs[1].Dimensions[0];
chain_result = new List <TMPoint>();
Array mat_chain_result = outs[1].ToArray();
for (int i = 0; i < ptos_brazos; i++)
{
var p = new TMPoint((double)mat_chain_result.GetValue(i, 0), (double)mat_chain_result.GetValue(i, 1));
chain_result.Add(p);
}
return(distance <= epsilon);
}
public TRNode(TMPoint p)
: this(p, null, null, new TRRange(p.Y, p.Y))
{
}
