public void addNode(RRTNode node)
{
int x = (int)node.X() - 25;
int y = 400 - (int)node.Z();
Cv2.Circle(pic, x, y, 2, Scalar.Blue, -1, LineTypes.Link8);
int fatherX = (int)node.Father().X() - 25;
int fatherZ = 400 - (int)node.Father().Z();
Cv2.Line(pic, x, y, fatherX, fatherZ, Scalar.Green);
}
private List <RRTNode> findPathOnRRT(RRTNode curr, RRTNode dest) // curr: current position, dest: destination
{
if (curr == dest)
{
return(new List <RRTNode>());
}
List <RRTNode> ancestors = new List <RRTNode>();
RRTNode r = dest;
bool b = true;
ancestors.Add(r);
while (b) // dont directly use "true", because visual studio will report an unreasonable error
{
if (r == root)
{
break;
}
r = r.Father();
if (r == curr)
{
ancestors.Reverse();
return(ancestors);
}
ancestors.Add(r);
}
List <RRTNode> path = new List <RRTNode>();
r = curr;
while (b)
{
r = r.Father();
path.Add(r);
List <RRTNode> commonParentToDest = new List <RRTNode>();
for (int i = 0; i < ancestors.Count; i++)
{
if (ancestors[i] == r)
{
for (int j = i - 1; j >= 0; j--)
{
commonParentToDest.Add(ancestors[j]);
}
foreach (RRTNode node in commonParentToDest)
{
path.Add(node);
}
b = false; // dont directly use "break", because visual studio will report an unreasonable error
}
else
{
commonParentToDest.Clear();
}
}
}
return(path);
}