public List <TMNode> follow_segment(TMSegment S) {//function Delta = followSegment(D, T, S) var Delta = new List <TMNode>(); //% Puntos extremos del segmento. TMPoint p = S.Point_1; TMPoint q = S.Point_2; var A_ = (TMANode)this.Root.buscar(p, S); Delta.Add(A_); int j = 0; // mientras q esté a la derecha de la frontera derecha de A_ while (q.X > A_.Trap.RightP.X) { // Si el rightp(A_) está por encima de S(i) TMANode A_1 = A_.Trap.RightP.Y > S.eval(A_.Trap.RightP.X) ? lower_right(A_) : upper_right(A_); if (A_1 == null) { break; } Delta.Add(A_1); A_ = A_1; j++; } return(Delta); }
public double DistanicaCon(TMPoint p) { double dx = this.x - p.x; double dy = this.y - p.y; return(Math.Sqrt(dx * dx + dy * dy)); }
public List <TRNode> GetThinPartition(double eps) { List <TRNode> ext = null; if (this.current != null && this.current.Count > 0) { ext = new List <TRNode>(); ext.Add(this.current[0]); TRNode node_actual = this.current[0]; for (int i = 1; i < this.current.Count; i++) { TMSegment puente = new TMSegment(node_actual.Point, this.current[i].Point); double dx = hipotenusa(puente, eps); int k = 1; while (node_actual.Point.X + k * dx < this.current[i].Point.X) { double inter_x = node_actual.Point.X + k * dx; TMPoint p_intermedio = new TMPoint(inter_x, puente.eval(inter_x)); ext.Add(new TRNode(p_intermedio)); k++; } ext.Add(new TRNode(this.current[i].Point)); node_actual = this.current[i]; } } return(ext); }
private TMANode upper_right_recursivo(TMANode A, TMNode D) { if (D is TMANode) { TMANode D_ = (TMANode)D; TMPoint a = A.Trap.RightP; TMPoint l = D_.Trap.LeftP; TMPoint bl = D_.Trap.Top_Left; TMPoint bla = A.Trap.Top_Right; // el trapecio lower_right vecino de A es el que tenga como leftp, el // mismo punto de rightp de A, pero además sea su top_left. if (a.Equals(l) && bl.Equals(bla)) { return(D_); } else { return(null); } } else { TMANode T = upper_right_recursivo(A, D.Node_Left); if (T == null) { T = upper_right_recursivo(A, D.Node_Rigth); } 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); }
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); }
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; }
public bool porEncimaDe(TMPoint p) { double y_ = Pendiente * p.X + N; // Calcular la diferencia. Si esta es negativa, entonces el // segmento está por encima del punto, si no, estará por debajo. double dlt = p.Y - y_; return(dlt < 0); }
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; }
public TMSegment(TMPoint p, TMPoint q) { if (p < q) { this.p1 = p; this.p2 = q; } else { this.p1 = q; this.p2 = p; } }
public override TMNode buscar(TMPoint p) { if (seg.porDebajoDe(p)) { return(this.node_left.buscar(p)); } else { 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)); } else { return(this.node_left.buscar(p, s)); } }
public override bool Equals(object obj) { if (obj is TMPoint) { TMPoint p = (TMPoint)obj; bool r = Math.Abs(this.x - p.x) < Utils.Eps && Math.Abs(this.y - p.y) < Utils.Eps; return(r); } else { return(false); } }
public bool PointInside(TMPoint p) { //int segs = points.Count; //segs = max(size(P.Segs)); int cayo_dentro = 0; //cayo_dentro = 0; int esta_entre = 0; //esta_entre = 0; foreach (TMSegment s in this.Segments) //for i=1:segs { int edos = entredos(s, p); //edos = entredos(P.Segs(i), p); cayo_dentro += Math.Abs(edos); //cayo_dentro = cayo_dentro + abs(edos); esta_entre += edos; //esta_entre = esta_entre + edos; } return(cayo_dentro > 0 && esta_entre == 0); //r = cayo_dentro > 0 && esta_entre == 0; }
/// <summary> /// Los puntos deben estar ordenados siguiendo el oredn inverso de las manesillas del reloj. /// comenzando por el de menor coordenada y (menor x en caso de empate) /// </summary> /// <param name="id"></param> /// <param name="x"></param> /// <param name="y"></param> public TMPoligon(int id, double[] x, double[] y) { int n = x.Length; if (x == null || y == null || n != y.Length) { throw new InvalidOperationException("Debe pasar la misma cantidad de absisas que de ordenadas."); } this.points = new List <TMPoint>(); for (int i = 0; i < n; i++) { var p = new TMPoint(x[i], y[i], id); points.Add(p); } }
/// <summary> /// Metodo privado /// </summary> /// <param name="S"></param> /// <param name="p"></param> /// <returns>-1: si la recta está por debajo del punto, /// 1 si la recta está por encima del punto, /// 0 si el punto pertenece a la recta /// </returns> private static int entredos(TMSegment S, TMPoint p) { int r = 0; //r = 0; //if (S.p1.x <= p.x && S.p2.x >= p.x) || (S.p2.x <= p.x && S.p1.x >= p.x) if ((S.Point_1.X <= p.X && S.Point_2.X >= p.X) || (S.Point_2.X <= p.X && S.Point_1.X >= p.X)) { double yr = S.eval(p.X); // yr = evalua_recta(S, p.x); double rest = yr - p.Y; // rest = yr - p.y; if (Math.Abs(rest) >= Utils.Eps) // if abs(rest) >= eps { r = (int)(rest / Math.Abs(rest)); // r = rest/abs(rest); } } return(r); }
private static TMANode lower_right_recursivo(TMANode A, TMNode D) { if (D is TMANode) { var D_ = (TMANode)D; TMPoint a = A.Trap.RightP; TMPoint l = D_.Trap.LeftP; TMPoint bl = D_.Trap.Bottom_Left; TMPoint bla = A.Trap.Bottom_Right; // el trapecio lower_right vecino de A es el que tenga como leftp, el // mismo punto de rightp de A, pero además sea su top_left. if (a.Equals(l) && bl.Equals(bla)) { return(D_); } return(null); } TMANode T = lower_right_recursivo(A, D.Node_Left) ?? lower_right_recursivo(A, D.Node_Rigth); return(T); }
public override TMNode buscar(TMPoint p, TMSegment s) { if (seg.porDebajoDe(p)) { return(this.node_left.buscar(p, s)); } else if (seg.porEncimaDe(p)) { return(this.node_rigth.buscar(p, s)); } else { if (s.Pendiente < seg.Pendiente) { return(this.node_rigth.buscar(p, s)); } else { return(this.node_left.buscar(p, s)); } } }
protected void initGraph(TMSearchGraph D, TMPoint end_efector) { TMANode tr_inicio = (TMANode)D.Root.buscar(end_efector); init_recursivo(null, D, tr_inicio); }
public TMPNode(TMPoint p) { this.point = p; }
public bool alaIzquierdaDe(TMPoint p) { return(p.alaDerechaDe(this)); }
public bool alaDerechaDe(TMPoint p) { return(this.x > p.x); }
public TRNode(TMPoint p, TRRange r) : this(p, null, null, r) { }
public TRNode(TMPoint p) : this(p, null, null, new TRRange(p.Y, p.Y)) { }
public abstract TMNode buscar(TMPoint p, TMSegment s);
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); } }
public void actualiza_grafo_busqueda(List <TMNode> Delta, TMSegment S)//, out TMNode primero) // el primero ese hay que eliminarlo { //primero = null; TMTrapezoid A1 = null, A2 = null; List <TMNode> a1_padres = null, a2_padres = null; for (int i = 0; i < Delta.Count; i++) { // Trapecio actual a analizar TMTrapezoid T = ((TMANode)Delta[i]).Trap; // Nodo actual a analizar TMNode Delta_node = Delta[i]; // Conjunto de padres del nodo actual List <TMNode> padres = Delta[i].Fathers; #region left-end-poit de Si dentro del primer trapecio if (i == 0 && S.Point_1.X > T.LeftP.X) { /* * En este bloque se construirá un trapecio a la izquierda del punto * y otro a la derecha. El izquierdo se añadirá al grafo de busqueda * y se proseguirá como si el derecho fuera el trapecio (nodo) actual */ // Dividiendo el trapecio actual TMPoint leftp_A = T.LeftP; TMPoint rightp_A = S.Point_1; TMPoint top_left_A = T.Top_Left; var top_rigth_A = new TMPoint(S.Point_1.X, T.Top.eval(S.Point_1.X), T.Top_Left.ID); TMPoint bottom_left_A = T.Bottom_Left; var bottom_right_A = new TMPoint(S.Point_1.X, T.Bottom.eval(S.Point_1.X), T.LeftP.ID); // Creando el nodo P que tendrá a la derecha el nuevo trapecio actual // y a la izquierda el que no se analizará mas TMNode P = new TMPNode(S.Point_1); var A = new TMTrapezoid(leftp_A, rightp_A, top_left_A, top_rigth_A, bottom_left_A, bottom_right_A); P.Node_Left = new TMANode(A); if (padres != null) { foreach (TMNode padre in padres) { // El nodo padre en la posicion que esta Delta_node, pon a P padre[padre.node_type(Delta_node)] = P; } padres = new List <TMNode> { P }; }// else Hay que ver el caso de que Delta sea el primer trapecio, que este: else { // luego se conectará con Q y se retornarán a traves de "primero" padres = new List <TMNode> { P }; this.Root = P; } // Dejar en T en trapecio que se analizará ahora. T = new TMTrapezoid(S.Point_1, T.RightP, top_rigth_A, T.Top_Right, bottom_right_A, T.Bottom_Right); // El ndodo actual es el trapecio de la derecha Delta_node = new TMANode(T); P.Node_Rigth = Delta_node; } #endregion #region right-end-point de Si dentro del ultimo trapecio if (i == Delta.Count - 1 && S.Point_2.X < T.RightP.X) { /* * En este bloque se construirá un trapecio a la izquierda del punto * y otro a la derecha. El derecho se añadirá al grafo de busqueda * y se proseguirá como si el izquierdo fuera el trapecio (nodo) actual */ TMPoint rightp_D = T.RightP; TMPoint leftp_D = S.Point_2; TMPoint top_right_D = T.Top_Right; var top_left_D = new TMPoint(S.Point_2.X, T.Top.eval(S.Point_2.X), T.Top_Right.ID); TMPoint bottom_right_D = T.Bottom_Right; var bottom_left_D = new TMPoint(S.Point_2.X, T.Bottom.eval(S.Point_2.X), T.Bottom_Right.ID); TMNode Q = new TMPNode(S.Point_2); var D = new TMTrapezoid(leftp_D, rightp_D, top_left_D, top_right_D, bottom_left_D, bottom_right_D); Q.Node_Rigth = new TMANode(D); if (padres != null) { foreach (TMNode padre in padres) { // El nodo padre en la posicion que esta Delta_node, pon a Q padre[padre.node_type(Delta_node)] = Q; } padres = new List <TMNode> { Q }; }// else Hay que ver el caso de que Delta sea el primer trapecio, que es este: else { padres = new List <TMNode> { Q }; //padre.Node_Rigth = Q; } // Dejar en T en trapecio que se analizará ahora. T = new TMTrapezoid(T.LeftP, S.Point_2, T.Top_Left, top_left_D, T.Bottom_Left, bottom_left_D); Delta_node = new TMANode(T); Q.Node_Left = Delta_node; } #endregion // El trapecio actual se remplazará por el segmento S // Hay que actualizar ese cambio en todos los padres del trapecio var Sn = new TMSNode(S); foreach (TMNode padre in padres) { padre[padre.node_type(Delta_node)] = Sn; } //Delta_node = Sn; // A1 y A2 son los trapecios que se iran construyendo mientras pasamos por los trapecios DELTA // La informacion d ela izquierda es la que se llena al principio (aqui) // Cuando ya un trapecio esta completo, se le actualiza la parte derecha en (*) if (A1 == null || A2 == null) { TMPoint top_left_arriba = T.Top_Left; TMPoint top_right_arriba = T.Top_Right; TMPoint intercept_left = (T.LeftP != null && T.LeftP.Equals(S.Point_1)) ? T.LeftP : new TMPoint(T.LeftP.X, S.eval(T.LeftP.X), S.Point_1.ID); TMPoint intercept_right = (T.RightP != null && T.RightP.Equals(S.Point_2)) ? T.RightP : new TMPoint(T.RightP.X, S.eval(T.RightP.X), S.Point_1.ID); TMPoint bottom_left_abajo = T.Bottom_Left; TMPoint bottom_right_abajo = T.Bottom_Right; TMPoint leftp_arriba = (intercept_left.Y > T.LeftP.Y) ? null : T.LeftP; //intercept_left : T.LeftP; TMPoint leftp_abajo = (intercept_left.Y < T.LeftP.Y) ? null : T.LeftP; //intercept_left : T.LeftP; TMPoint rightp_arriba = (intercept_right.Y > T.RightP.Y) ? null : T.RightP; //intercept_right : T.RightP; TMPoint rightp_abajo = (intercept_right.Y < T.RightP.Y) ? null : T.RightP; //intercept_right; if (A1 == null) { A1 = new TMTrapezoid(leftp_arriba, rightp_arriba, top_left_arriba, top_right_arriba, intercept_left, intercept_right); a1_padres = new List <TMNode>(); } if (A2 == null) { A2 = new TMTrapezoid(leftp_abajo, rightp_abajo, intercept_left, intercept_right, bottom_left_abajo, bottom_right_abajo); a2_padres = new List <TMNode>(); } } // En este bloque se verifica que trapecio está completo ya bool a1_completo = (A1.LeftP != null && ((S.porDebajoDe(A1.LeftP) || A1.LeftP.Equals(S.Point_1)) && (S.porDebajoDe(T.RightP) || T.RightP.Equals(S.Point_2)))); bool a2_completo = (A2.LeftP != null && ((S.porEncimaDe(A2.LeftP) || A2.LeftP.Equals(S.Point_1)) && (S.porEncimaDe(T.RightP) || T.RightP.Equals(S.Point_2)))); if (a1_completo) //(*) { A1.RightP = T.RightP; A1.Top_Right = T.Top_Right; A1.Bottom_Right = new TMPoint(T.RightP.X, S.eval(T.RightP.X), S.Point_2.ID); a1_padres.Add(Sn); // Asignarlo como hijo izquierdo del nodo correspondiente; var A1_node = new TMANode(A1); foreach (TMNode padre in a1_padres) { padre.Node_Left = A1_node; } A1 = null; } else { a1_padres.Add(Sn); } if (a2_completo) //(*) { A2.RightP = T.RightP; A2.Bottom_Right = T.Bottom_Right; A2.Top_Right = new TMPoint(T.RightP.X, S.eval(T.RightP.X), S.Point_2.ID); a2_padres.Add(Sn); // Asignarlo como hijo derecho del nodo correspondiente; var A2_node = new TMANode(A2); foreach (TMNode padre in a2_padres) { padre.Node_Rigth = A2_node; } A2 = null; } else { a2_padres.Add(Sn); } } }
public override TMNode buscar(TMPoint p, TMSegment s) { return(this); }
public abstract TMNode buscar(TMPoint p);
public override TMNode buscar(TMPoint p) { return(this); }