Algorithm 1.

$\mathcal{T}\leftarrow \text{BUILD}\_\text{NEEDLE}\_\mathrm{RRT}(q_{\mathrm{init}},Q_{\text{goal}})$

1:	$\mathcal{T}\leftarrow \text{INITIALIZE}\_\text{TREE}\left(q_{\mathrm{init}}\right)$
2:	while ($\mathcal{T}\cap Q_{\text{goal}}=\varnothing$ and k < K) do
3:	repeat
4:	prand ← RANDOM_POINT_R3()
5:	Qreach = ∅
6:	for all $q_i\in \mathcal{T}$ do
7:	if REACHABLE(prand, qi) then
8:	Qreach ← Qreach ⋃ qi
9:	end if
10:	end for
11:	until Qreach = ∅
12:	qnear ← NEAREST_NEIGHBOR_R3(prand,Qreach)
13:	$u\leftarrow \text{SOLVE}\_\text{PARAMETERS}(q\text{near},p_{\text{rand}},\mathcal{T})$
14:	qnew ← NEW_STATE(qnear, u)
15:	if VALID_EDGE(qnear, qnew, u) then
16:	$\mathcal{T}\leftarrow \text{ADD}\_\text{VERTEX}\left(q_{\text{new}}\right)$
17:	$\mathcal{T}\leftarrow \text{ADD}\_\text{EDGE}(q_{\text{near}},q_{\text{new}},u)$
18:	end if
19:	k ← k + 1
20:	end while
21:	return $\mathcal{T}$