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