Algorithm 4 M-RRT*
1:	G ← (V, E); V ← xinit; xpeak ← xinit; N ← 0;
2:	$\widehat{h}$(xpeak) ← Dis(xpeak, xgoal);
3:	$\widehat{h}$(xinit) ← Dis(xinit, xgoal);
4:	for i = 1 to n do
5:	xrand ← Sample(i);
6:	xnearest ← Nearest(V, xrand);
7:	(xn, σ) ← Steer(xnearest, xrand);
8:	if CollisionFree(σ) then
9:	if N = 0 then
10:	AVAC(xn, $\widehat{h}$(xpeak), $\widehat{h}$(xinit));
11:	else
12:	NDVAC(xn, xnearest, n, N, σ*);
13:	Xnear ← Near(V, xn);
14:	(xparent, σparent) ← ChooseParent(Xnear, xnearest, xn, σ);
15:	V ← AddVertex(xn);
16:	E ← AddEdge(xparent, xn);
17:	G ← Rewire(G, xn, Xnear);
18:	(σ*, N) ← NearGoal(G, xn, n);
19:	end if
20:	end for
21:	return G;