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>
/// Construir la informacion del padre que tendrá en el futuro
/// </summary>
/// <param name="padres"></param>
/// <param name="hijo"></param>
public TMFamilia(List <TMNode> padres, TMNode node)
{
this.padres = padres;
this.posiciones = new List <NodeType>();
for (int i = 0; i < padres.Count; i++)
{
posiciones.Add(padres[i].node_type(node));
}
}
/// <summary>
/// Construir la informacion del padre que tendrá en el futuro
/// </summary>
/// <param name="padres"></param>
/// <param name="hijo"></param>
/// <param name="node"></param>
public TMFamilia(List <TMNode> padres, TMNode node)
{
this.padres = padres;
this.posiciones = new List <NodeType>();
foreach (TMNode t in padres)
{
posiciones.Add(t.node_type(node));
}
}
private static void lista_trapecios_recursivo(TMNode node, List <TMTrapezoid> trapecios)
{
if (node is TMANode)
{
trapecios.Add(((TMANode)node).Trap);
}
else
{
lista_trapecios_recursivo(node.Node_Left, trapecios);
lista_trapecios_recursivo(node.Node_Rigth, trapecios);
}
}
//protected NodeType node_type;
//public NodeType node_type
//{
// get { return this.node_type; }
//}
public NodeType node_type(TMNode node)
{
if (node.Equals(this.node_left))
{
return(NodeType.Left);
}
if (node.Equals(this.node_rigth))
{
return(NodeType.Right);
}
return(NodeType.None);
}
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 TMNode this[NodeType s]
{
get
{
switch (s)
{
case NodeType.Left:
return(this.node_left);
case NodeType.Right:
return(this.node_rigth);
case NodeType.None:
return(null);
default:
return(null);
}
}
set
{
switch (s)
{
case NodeType.Left:
this.Node_Left = value;
break;
case NodeType.Right:
this.Node_Rigth = value;
break;
case NodeType.None:
break;
default:
break;
}
}
}
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 TMSearchGraph(TMNode root)
{
this.Root = root;
}
/// <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);
}