public void addRootNode(RRTNode node)
{
int x = (int)node.X() - 25;
int y = 400 - (int)node.Z();
Cv2.Circle(pic, x, y, 2, Scalar.Blue, -4, LineTypes.Link8);
}
public RRTNode(float theX, float theZ, RRTNode theFather)
{
x = theX;
z = theZ;
father = theFather;
distanceToFather = Mathf.Sqrt(Mathf.Pow(father.X() - this.x, 2) + Mathf.Pow(father.Z() - this.z, 2));
nodeID = z * 500 + x;
}
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);
}
public float distanceTo(RRTNode other)
{
float distanceSquare = Mathf.Pow(other.X() - x, 2) + Mathf.Pow(other.Z() - z, 2);
return(Mathf.Sqrt(distanceSquare));
}