# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 4 Derivatives of the Optimal Lengths
### Derivatives of the LRL and RLR Cases  
Using equation (33) again, we have  
\[2\ell_{2}^{\text{CCC}}=\ell_{1}^{\text{CCC}}+\ell_{2}^{\text{CCC}}+\ell_{3}^{ \text{CCC}}-s(\beta-\alpha)\mod(2\pi), \tag{43}\]  
and thus, deriving (43), we obtain  
\[2\partial_{\alpha}\ell_{2}^{\text{CCC}}=\partial_{\alpha}\ell^{\text{CCC}}+s,\qquad 2\partial_{\beta}\ell_{2}^{\text{CCC}}=\partial_{\beta}\ell^{\text{ CCC}}-s. \tag{44}\]  
Then, from (44) and in recalling Equation (33) and the expression of \(t^{\text{CCC}}\) from (27),  
\[\partial_{\alpha}(\ell^{\text{CCC}}) =2\partial_{\alpha}(\ell_{2}^{\text{CCC}})-s=\pm\frac{\sin(\alpha -\beta)+s\,d\cos\alpha}{2\sqrt{1-(t^{\text{CCC}})^{2}}}-s, \tag{45}\] \[\partial_{\beta}(\ell^{\text{CCC}}) =2\partial_{\alpha}(\ell_{2}^{\text{CCC}})+s=s\mp\frac{\sin( \alpha-\beta)+s\,d\cos\beta}{2\sqrt{1-(t^{\text{CCC}})^{2}}}.\]  
The sign depends on whether the first or the second solution for \(\ell_{2}\) is used. The results in (38), (42) and (45) are compactly implemented in Algorithm 1, which solves the classic point-to-point Dubins problem and returns the solution and its derivatives. This is instrumental for building the algorithm that solves the 3 PDP presented in the next section. ```
1:functionDubinsCSCp(\(d\),\(\alpha\),\(\beta\))
2:\(\mathcal{S}^{+}\leftarrow\sin\alpha+\sin\beta\); \(C^{+}\leftarrow\cos\alpha+\cos\beta\); (6)
3:for\(s\in\{-1,1\}\)do
4:for\(X\) real root of \((2-\,C^{+})\,X^{2}+2(s\,d+\,S^{+})\,X+(2+\,C^{+})=0\)do
5:\(\theta_{S}\leftarrow\text{atan2}(-2X,X^{2}-1)\) (12) \(\ell\leftarrow\ell_{1}+\ell_{2}+\ell_{3}\) (7) \(\ell_{1}\leftarrow\ell(\delta_{S}-\alpha)\mod(2\pi)\) (19) \(\theta_{\Delta}\ell\gets s(\cos(\theta_{S}-\alpha)-1)\) (42) \(\theta_{\Delta}\ell\gets s(\cos(\theta_{S}-\beta)-1)\) (42)
6:endfor
7:endfor
8: Select the solution with minimal \(\ell\) or set \(\ell_{1}=\ell_{2}=\ell_{3}=\infty\) in case no solution exists. 9:return\(\ell_{1},\ell_{2},\ell_{3},s,0,-s,\partial_{\alpha}\ell,\partial_{\beta}\ell\);
10:endfunction
11:functionDubinsCCC(\(d\),\(\alpha\),\(\beta\))
12:\(S^{-}\leftarrow\sin\alpha-\sin\beta\); (6)
13:for\(s\in\{-1,1\}\)do
14:\(t\leftarrow(3+\cos(\alpha-\beta)-s\,d\,S^{-})/4-d^{2}/8\) (31)
15:if\(t\in[-1,1]\)then
16:for\(\ell_{2}\in\{\arccos(t),2\pi-\arccos(t)\}\) (32)do
17: solve linear system (30) and compute
18:\(\ell_{1}\leftarrow\text{atan2}(\sin\ell_{1},\cos\ell_{1})\mod 2\pi\) (33);
19:\(\ell_{3}\leftarrow\text{atan2}(\sin\ell_{3},\cos\ell_{3})\mod 2\pi\) (33);
20:\(\partial_{\alpha}\ell\leftarrow\pm(\sin(\alpha-\beta)+s\,d\cos\alpha)/(2\sqrt{1- t^{2}})-s\) (45)
21:\(\partial_{\beta}\ell\gets s\mp(\sin(\alpha-\beta)+s\,d\cos\beta)/(2\sqrt{1- t^{2}})\) (45)
22:endfor
23:endif
24:ifnot\(\alpha+s(\ell_{1}-\ell_{2}+\ell_{3})=\beta\) mod \(2\pi\) (34) (35)then
25: Discard the solution
26:endif
27:endfor
28:return\(\ell_{1},\ell_{2},\ell_{3},s,-s,\partial_{\alpha}\ell,\partial_{\beta}\ell\);
29:endfunction
```  
**Algorithm 1** Dubin's solver with derivatives  
```
1:functionDubinsCSCp(\(d\),\(\alpha\),\(\beta\))
2:\(\mathcal{S}^{+}\leftarrow\sin\alpha+\sin\beta\); \(C^{+}\leftarrow\cos\alpha+\cos\beta\); (6)
3:for\(s\in\{-1,1\}\)do
4:for\(X\) real root of \((2-\,C^{+})\,X^{2}+2(s\,d+\,S^{+})\,X+(2+\,C^{+})=0\)do
5:\(\theta_{S}\leftarrow\text{atan2}(-2X,X^{2}-1)\) (12) \(\ell\leftarrow\ell_{1}+\ell_{2}+\ell_{3}\) (7) \(\ell_{1}\leftarrow\ell(\delta_{S}-\alpha)\mod(2\pi)\) (19) \(\ell_{2}\leftarrow(d+s\,S^{+})\cos\theta_{S}-s\,C^{+}\sin\theta_{S}\) (19) \(\theta_{\Delta}\ell\gets s(\cos(\theta_{S}-\alpha)-1)\) (42) \(\theta_{\Delta}\ell\gets s(\cos(\theta_{S}-\beta)-1)\) (42)
6:endfor
7:endfor
8: Select the solution with minimal \(\ell\) or set \(\ell_{1}=\ell_{2}=\ell_{3}=\infty\) in case no solution exists.