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