Algorithm 2 ChooseParent(Xnear, xnearest, xnew, σnearest)
1:	xmin ← xnearest;
2:	σmin ← σnearest;
3:	cmin ← Cost(xmin) + Cost(σmin);
4:	for each xnear ∈ Xnear do
5:	σ ← Connect(xnear, xnew);
6:	c ← Cost(xnear) + Cost(σ);
7:	if c < cmin then
8:	if CollisionFree(σ) then
9:	xmin ← xnear;
10:	σmin ← σ;
11:	cmin ← c;
12:	end if
13:	end if
14:	end for
15:	return (xmin, σmin);