# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 3 Derivation of Dubins Path with Trigonometry  
This section is devoted to deriving the solution of the classic Dubins problem to obtain analytic relations for the optimal lengths so that the next section can compute their derivatives. Without loss of generality, the original problem (1) can be transformed into a simpler one by translating, rotating, and scaling the original configuration such that \(x_{i}=y_{i}=0\), and \(x_{f}=d,y_{f}=0\) with a unitary maximum curvature of \(\kappa_{\max}=1\). Let \(d\) be the Euclidean distance between the initial and final points scaled by the maximum curvature:  
\[d=\sqrt{(x_{f}-x_{i})^{2}+(y_{f}-y_{i})^{2}}/\kappa_{\max}. \tag{2}\]  
Then, the aforementioned transform is the following, (3): the translation is given by \(-(x_{i},y_{i})\) and is followed by a rotation around \((0,0)\) by an angle \(-\phi\) given by \(\phi=\text{atan}2(y_{f}-y_{i},x_{f}-x_{i})\), followed by a scaling of \(\kappa_{\max}^{-1}=\rho_{\min}\), or compactly,  
\[ \begin{bmatrix}x\\ y\end{bmatrix} =\frac{1}{\kappa_{\max}} \begin{bmatrix}\cos\phi&\sin\phi\\ -\sin\phi&\cos\phi\end{bmatrix}\begin{bmatrix}x-x_{i}\\ y-y_{i}\end{bmatrix} . \tag{3}\]  
Let \(\alpha=\vartheta_{i}+\phi\) and \(\beta=\vartheta_{f}+\phi\) be the initial and final angles in the transformed coordinate system. The new initial point is thus the origin with angle \(\alpha\), and the final point is on the positive \(x-\)axis at distance \(d\), (2), from the origin and angle  
The solution to a Dubins problem is a sequence of three curves (lines or arcs [32]) that can be written in the following form for \(j\in 0,1,2\):  
\[j-\text{th curve:}\quad\left\{ \begin{array}{l}x(\ell)=x_{j}+s(\sin(\theta_{j}+s \ell)-\sin\theta_{j})+(1-s^{2})\ell\cos\theta_{j},\\ y(\ell)=y_{j}+s(\cos\theta_{j}-\cos(\theta_{j}+s\ell))+(1-s^{2})\ell\sin \theta_{j},\\ \theta(\ell)=\theta_{j}+s\ell,\end{array} \right. \tag{4}\]  
where \(\ell\) is the curvilinear abscissa, \(x(\ell)\) and \(y(\ell)\) are the coordinates, and \(\theta(\ell)\) is the orientation. Let us define \(x_{0}=y_{0}=0\) and \(\theta_{0}=\alpha\), corresponding to the initial point. Notice that, when the integer variable \(s\) is equal to \(0\), the function represents a line segment, whereas, when \(s=1\), the curve is an arc of a unitary circle turning left, and, when \(s=-1\), the circle is turning right. The Dubins problem is formulated with three internal segments (CSC or CCC) with known initial and final coordinates and headings in the transformed coordinate system. Hence, we can juxtapose three of such curves. Using the notation \(s_{1}\), \(s_{2}\), and \(s_{3}\) for the signs of the curvature of the three segments coming from (4), we can write the equations for the three segments:  
\[ \begin{split} x_{1}&=\,0+s_{1}(\sin\theta_{1}-\sin \alpha)\\ y_{1}&=\,0+s_{1}(\cos\alpha-\cos\vartheta_{1})\\ \vartheta_{1}&=\,\alpha+s_{1}\ell_{1}\\ x_{2}&=\,x_{1}+s_{2}(\sin\theta_{2}-\sin\vartheta_{1})+(1-s _{2}^{2})\ell_{2}\cos\vartheta_{1}\\ y_{2}&=\,y_{1}+s_{2}(\cos\vartheta_{1}-\cos\theta_{2})+(1-s _{2}^{2})\ell_{2}\sin\vartheta_{1}\\ \vartheta_{2}&=\,\vartheta_{1}+s_{2}\ell_{2}\\ 0&=\,x_{2}+s_{3}(\sin\beta-\sin\vartheta_{2})\\ d&=\,y_{2}+s_{3}(\cos\vartheta_{2}-\cos\beta)\\ \beta&=\,\vartheta_{2}+s_{3}\ell_{3}.\end{split}  \tag{5}\]  
The proposed transformation simplifies the equations, eliminating \(x_{1}\), \(x_{2}\), \(y_{1}\), \(y_{2}\), \(\theta_{1}\), and \(\theta_{2}\) from the general system (5). Moreover, we can separate two families of solutions based on the presence or absence of the middle singular arc (straight line segment), that is, the CSC and CCC cases. Before proceeding, it is also convenient to define some notation and shorthand for the trigonometric functions of the angles \(\alpha\) and \(\beta\) that will be used in the following sections, to specify that the expression depends on trigonometric combinations of \(\alpha\) and \(\beta\):  
\[\left\{