Algorithm 2 Transformation from workspace to C-space.
1: functionC-space($start,goal$, $a_{j,k}$, $v,L_v$)
2:     $j,k$	▹ Temporary variables
3:     for $(j=0;j<v;j++)$ do
4:         $w_{start}\left[j\right]=arccos\left(\frac{start[j+1]-start\left[j\right]}{L\left[j\right]}\right)$	▹ Start position in C-space.
5:         $w_{goal}\left[j\right]=arccos\left(\frac{goal[j+1]-goal\left[j\right]}{L\left[j\right]}\right)$	▹ Goal position in C-space.
6:     end for
7:     for $(j=0;j<v;j++)$do	▹ Calculation of the value of $a_0$.
8:         for $(k=0;k<v;k++)$ do
9:            $a_0\left[j\right]=\left(w_{goal}[k+1]\right)\left(a\left[j\right]\left[k\right]\right)$
10:         end for
11:         $a_0\left[j\right]-=w_{goal}\left[0\right]$
12:     end for
13:     return $(w_{start},w_{goal},a_0)$	▹ Returns the values from the C-space
14: end function