Algorithm 6. ChooseParent(Snear, nnew, nnearest)
1	nmin ← nnearest;
2	Cmindist ← C(nnear) + Dist(nnear, nnew);
3	for each nnear ∈ Snear do
4	Cdist ← C(nnear) + Dist(nnear, nnew);
5	if Collision-free(nnear, nnew, Sobs) then
6	if Cmindist < Cdist then
7	Cmindist = Cdist;
8	nmin ← nnear;
9	end if;
10	end if;
11	end for
12	Return nmin;