# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 3 Derivation of Dubins Path with Trigonometry
### Case CCC  
The CCC case, happening when \(s_{2}\
eq 0\), comprises two paths: LRL and RLR. In this situation, we can impose \(s_{1}=s_{3}=-s_{2}=s\) and simplify (5) to obtain  
\[ \begin{split} s\,d&=\,(\sin\theta_{1}-\sin\alpha)-( \sin\theta_{2}-\sin\theta_{1})+(\sin\beta-\sin\theta_{2})\\ 0&=\,(\cos\alpha-\cos\theta_{1})-(\cos\theta_{1}- \cos\theta_{2})+(\cos\theta_{2}-\cos\beta).\end{split}  \tag{22}\]  
Figure 3: Example of the total length of a three-point Dubins path. Spatial coordinates are in meters, and angles are in radiant. (**Top**: path result with alternating colors for each arc; **Middle**: total length \(\ell\) as a function of middle angle \(\theta_{m}\); **Bottom**: derivative of total length \(\ell\) as a function of middle angle \(\theta_{m}\)). Equation (22) can be rewritten in compact form as  
\[ \begin{split} s\,d&=\,2(\sin\vartheta_{1}-\sin \vartheta_{2})+\sin\beta-\sin\alpha\\ 0&=\,2(\cos\vartheta_{2}-\cos\vartheta_{1})+\cos \alpha-\cos\beta\\ \beta&=\,\alpha+s(\ell_{1}-\ell_{2}+\ell_{3}).\end{split}  \tag{23}\]  
We can state equations (23) in terms of the parameters \(\mathcal{P}_{\alpha,\beta}\) using notation (6) as  
\[ \begin{split} s\,d&=\,2(\sin\vartheta_{1}-\sin \vartheta_{2})-\,\,\mathcal{S}^{-}\\ 0&=\,2(\cos\vartheta_{2}-\cos\vartheta_{1})+\, \,\mathcal{C}^{-}.\end{split}  \tag{24}\]  
From (24) and squaring, we obtain  
\[ \begin{split}\frac{1}{4}(s\,d+\,\mathcal{S}^{-})^{2}& =\,(\sin\vartheta_{1}-\sin\vartheta_{2})^{2}=\sin\vartheta_{1}^{2 }+\sin\vartheta_{2}^{2}-2\sin\vartheta_{1}\sin\vartheta_{2}\\ \frac{1}{4}(\,\mathcal{C}^{-})^{2}&=\,(\cos \vartheta_{2}-\cos\vartheta_{1})^{2}=\cos\vartheta_{1}^{2}+\cos\vartheta_{2} ^{2}-2\cos\vartheta_{1}\sin\vartheta_{2}\end{split}  \tag{25}\]  
and by adding the two equalities (25) (and dividing by 2), we obtain  
\[ \begin{split}\frac{(s\,d+\,\mathcal{S}^{-})^{2}+(\,\mathcal{C}^{- })^{2}}{8}&=\,1-\sin\vartheta_{1}\sin\vartheta_{2}-\cos \vartheta_{1}\sin\vartheta_{2}\\ &=\,1-\cos(\vartheta_{2}-\vartheta_{1})=1-\cos\ell_{2},\end{split}  \tag{26}\]  
where the last step follows inserting the sixth equation of (5), that is, \(\vartheta_{2}-\vartheta_{1}=s_{2}\ell_{2}\), and noting that cosine is even. Thus, \(\ell_{2}\) can be obtained from (26) as follows:  
\[ \begin{split} f^{\text{CCC}}&=\,\frac{3+\cos(\alpha- \beta)+s\,d(\sin\beta-\sin\alpha)}{4}-\frac{d^{2}}{8},\\ \cos\ell_{2}&=\,1-\frac{(s\,d+\,\mathcal{S}^{-})^{2 }+(\,\mathcal{C}^{-})^{2}}{8}=t^{\text{CCC}},\\ \ell_{2}&=\,\arccos(t^{\text{CCC}}),\quad\text{or} \quad\ell_{2}=2\pi-\arccos(t^{\text{CCC}}).\end{split}  \tag{27}\]  
The solution exists when the argument of arccos is in the interval \([-1,1]\). Therefore, the \(t^{CCC}\) solved in (27) must be comprised between \(-1\) and \(1\), yielding the following inequality:  
\[-1\leq\frac{3+\cos(\alpha-\beta)+s\,d(\sin\beta-\sin\alpha)}{4}-\frac{d^{2}}{8 }\leq 1. \tag{28}\]  
The previous expression (28) can be multiplied by 4 and moved \(-d^{2}/2+3\) to the left and right side:  
\[\frac{d^{2}}{2}-7\leq\cos(\alpha-\beta)+s\,d(\sin\beta-\sin\alpha)\leq\frac{d^{ 2}}{2}+1. \tag{29}\]  
We can obtain the other relations from the trigonometric relations furnished by (24). From \(\vartheta_{1}=\alpha+s\ell_{1}\) and \(\vartheta_{2}=\alpha+s(\ell_{1}-\ell_{2})\) and expanding the trigonometric functional, we obtain a linear system in \(\cos\ell_{1}\) and \(\sin\ell_{1}\), and from \(\vartheta_{1}=\beta+s(\ell_{2}-\ell_{3})\) and \(\vartheta_{2}=\beta-s\ell_{3}\) and expanding the trigonometric functions again, we obtain a linear system in \(\cos\ell_{3}\) and \(\sin\ell_{3}\) as follows:\[ \begin{bmatrix}\sin\alpha-\sin\tilde{\alpha}&s(\cos\alpha-\cos\tilde{\alpha}) \end{bmatrix}\begin{bmatrix}\cos\ell_{1}\\ \sin\ell_{1}\end{bmatrix}  = \tag{30}\] \[ \begin{bmatrix}\sin\tilde{\beta}-\sin\beta&s(\cos\beta-\cos\tilde{ \beta})\\ \cos\tilde{\beta}-\cos\beta&s(\sin\tilde{\beta}-\sin\beta)\end{bmatrix} \begin{bmatrix}\cos\ell_{3}\\ \sin\ell_{3}\end{bmatrix}  =\] \[ \begin{bmatrix}s\,d+\,S^{-}\\ C^{-}\end{bmatrix}