/// <summary> /// PRUEBA.El universo se asume que es un poligono /// rectangular con los puntos ordenados comenzando por el extremos superior izquierdo y luego /// siguiendo el orden d elas manesillas del reloj. /// </summary> /// <param name="universe"></param> /// <returns></returns> public static List <TMTrapezoid> freeConfigurationSpacePrueba(TMPoligon universe, out TMNode sg) { //T = R; //% Se inicializa la estructura de búsqueda con el trapecio R como hoja. //D.node = 'hoja'; //D.object = R; TMSearchGraph D = GetInitPrueba(universe); List <TMSegment> S_ = new List <TMSegment>(); S_.Add(new TMSegment(new TMPoint(50, 50), new TMPoint(40, 60))); S_.Add(new TMSegment(new TMPoint(60, 60), new TMPoint(50, 50))); S_.Add(new TMSegment(new TMPoint(40, 60), new TMPoint(60, 60))); S_.Add(new TMSegment(new TMPoint(30, 100), new TMPoint(70, 100))); S_.Add(new TMSegment(new TMPoint(70, 100), new TMPoint(90, 80))); S_.Add(new TMSegment(new TMPoint(90, 80), new TMPoint(10, 80))); S_.Add(new TMSegment(new TMPoint(10, 80), new TMPoint(30, 100))); int n = S_.Count; //n = size(S); for (int i = 0; i < n; i++) //for i = 1: n { //% Encontrar el conjunto de trapecios que intersecta S(i) y eliminarlos de T List <TMNode> Delta = D.follow_segment(S_[i]); //Delta = followSegment(D, T, S(i)); D.actualiza_grafo_busqueda(Delta, S_[i]); //D = actualiza_grafo_busqueda(D, Delta, S(i)); }//end List <TMTrapezoid> T = D.lista_trapecios(); //T = lista_trapecios(D); sg = D.Root; return(T); }
/// <summary> /// /// </summary> /// <param name="D"></param> /// <param name="end_efector"></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, double seps) { this.stack = new Stack <TRNode>(); this.current = null; this.separacion = seps; initGraph(D, end_efector); }
private TMSearchGraph GetInit(TMPoligon universe) { TMTrapezoid trap = new TMTrapezoid(universe.GetPointAt(0), universe.GetPointAt(2), universe.GetPointAt(3), universe.GetPointAt(2), universe.GetPointAt(0), universe.GetPointAt(1)); TMANode a = new TMANode(trap); TMSearchGraph T = new TMSearchGraph(a); return(T); }
private static TMSearchGraph GetInit(TMPoligon universe) { var trap = new TMTrapezoid(universe.GetPointAt(0), universe.GetPointAt(2), universe.GetPointAt(3), universe.GetPointAt(2), universe.GetPointAt(0), universe.GetPointAt(1)); var a = new TMANode(trap); var T = new TMSearchGraph(a); return(T); }
/// <summary> /// Mapa trapezoidal de un sistema de obstaculos. El universo se asume que es un poligono /// rectangular con los puntos ordenados comenzando por el extremos superior izquierdo y luego /// siguiendo el orden d elas manesillas del reloj. /// </summary> /// <param name="universe"></param> /// <returns></returns> public List <TMTrapezoid> freeConfigurationSpace(TMPoligon universe, out TMSearchGraph sg) { //T = R; //% Se inicializa la estructura de búsqueda con el trapecio R como hoja. //D.node = 'hoja'; //D.object = R; TMSearchGraph D = GetInit(universe); //% Permutar random el conjunto de aristas. List <TMSegment> S_ = this.p_system.GetRandomPermutationOfSegments(); //S_ = permutar(S); int n = S_.Count; //n = size(S); for (int i = 0; i < n; i++) //for i = 1: n { //% Encontrar el conjunto de trapecios que intersecta S(i) y eliminarlos de T List <TMNode> Delta = D.follow_segment(S_[i]); //Delta = followSegment(D, T, S(i)); D.actualiza_grafo_busqueda(Delta, S_[i]); //D = actualiza_grafo_busqueda(D, Delta, S(i)); }//end List <TMTrapezoid> T = D.lista_trapecios_exteriores(); //T = lista_trapecios(D); //List<TMTrapezoid> T = D.lista_trapecios(); sg = D; return(T); }
protected void init_recursivo(TRNode anterior, TMSearchGraph D, TMANode tr) { TMPoint centro = tr.Trap.Centro; TRNode node_centro = 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; } 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; TMPoint pu = new TMPoint(xpu, ypu); TRNode node_upper = new TRNode(pu, rango_derecho(tr.Trap, ypu)); node_centro.Upper = node_upper; init_recursivo(node_upper, D, upper_right); } if (se_construye_el_lower) { double xpw = tr.Trap.RightP.X; double ypw = (tr.Trap.RightP.Y + tr.Trap.Bottom_Right.Y) / 2; TMPoint pw = new TMPoint(xpw, ypw); TRNode node_lower = new TRNode(pw, rango_derecho(tr.Trap, ypw)); node_centro.Lower = node_lower; init_recursivo(node_lower, D, lower_right); } }
protected void initGraph(TMSearchGraph D, TMPoint end_efector) { TMANode tr_inicio = (TMANode)D.Root.buscar(end_efector); init_recursivo(null, D, tr_inicio); }
public TREXT_TrajectoryGraph(TMSearchGraph D, TMPoint end_efector, double seps) : base(D, end_efector, seps) { }