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)) { }