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