protected void initGraph(TMSearchGraph D, TMPoint end_efector, TMPoint objetivo)
{
TMANode tr_inicio = (TMANode)D.Root.buscar(end_efector);
TMANode tr_fin = (TMANode)D.Root.buscar(objetivo);
init_recursivo(null, D, tr_inicio, tr_fin, objetivo);
}
protected void init_recursivo(TRNode anterior, TMSearchGraph D, TMANode tr, TMANode tr_fin, TMPoint objetivo)
{
TMPoint centro = tr.Trap.Centro;
TRNode node_centro = (anterior == null) ?
new TRNode(centro, new TRRange(centro.Y, centro.Y))
: 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;
}
if (tr.Equals(tr_fin))
{
//TRNode node_objetivo = new TRNode(objetivo, new TRRange(objetivo.Y, objetivo.Y));
//node_objetivo.IsObjetive = true;
//node_centro.Upper = node_objetivo;
var node_objetivo = new TRNode(objetivo, new TRRange(objetivo.Y, objetivo.Y))
{
IsObjetive = true
};
if (anterior.Upper.Equals(node_centro))
{
anterior.Upper = node_objetivo;
}
else
{
anterior.Lower = node_objetivo;
}
}
else
{
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;
var pu = new TMPoint(xpu, ypu);
var node_upper = new TRNode(pu, rango_derecho(tr.Trap, ypu));
node_centro.Upper = node_upper;
init_recursivo(node_upper, D, upper_right, tr_fin, objetivo);
}
if (se_construye_el_lower)
{
double xpw = tr.Trap.RightP.X;
double ypw = (tr.Trap.RightP.Y + tr.Trap.Bottom_Right.Y) / 2;
var pw = new TMPoint(xpw, ypw);
var node_lower = new TRNode(pw, rango_derecho(tr.Trap, ypw));
node_centro.Lower = node_lower;
init_recursivo(node_lower, D, lower_right, tr_fin, objetivo);
}
if (!se_construye_el_upper && !se_construye_el_lower)
{
node_centro.Y_Range = new TRRange(centro.Y, centro.Y);
}
}
}