# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 5 Three-Point Dubins Problem (3 PDP)  
In this section, we derive the formulation for the 3 PDP. The resulting Dubins spline will be a concatenation of two Dubins paths. Hence, the total length of such a spline will be the sum of the lengths of the two elementary Dubins paths:  
\[\ell_{\text{total}}(\mathbf{P_{i}},\mathbf{P_{m}},\mathbf{P_{f}},\vartheta_{i},\vartheta_{m},\vartheta_{f})=\ell_{\text{dubl}1}(\mathbf{P_{i}},\mathbf{P_{ m}},\vartheta_{i},\vartheta_{m})+\ell_{\text{dubl}2}(\mathbf{P_{m}},\mathbf{P_{ f}},\vartheta_{m},\vartheta_{f}), \tag{46}\]  
where \(\mathbf{P_{i}}=(x_{i},y_{i})\), \(\mathbf{P_{m}}=(x_{m},y_{m})\), and \(\mathbf{P_{f}}=(x_{f},y_{f})\) are the three points defining the 3 PDP path, \(\vartheta_{i}\) and \(\vartheta_{f}\) are the initial and final angles of the path, and finally, \(\vartheta_{m}\) is the angle of the middle point and the only unknown variable. Therefore, we will consider the total length \(\ell_{\text{total}}\) as a function of \(\vartheta_{m}\) only and also consider its derivative, \(\ell_{\text{total}}^{\prime}\). Hence, (46) can be rewritten as  
\[\ell_{\text{total}}(\vartheta_{m}) = \ell_{\text{dubl}1}(\vartheta_{m})+\ell_{\text{dubl}2}(\vartheta_ {m}), \tag{47}\] \[\ell_{\text{total}}^{\prime}(\vartheta_{m}) = \ell_{\text{dubl}1}^{\prime}(\vartheta_{m})+\ell_{\text{dubl}2}^{ \prime}(\vartheta_{m}).\]  
The derivatives are computed thanks to the results of the previous sections and, given (47), we have the following:  
\[\ell_{\text{dubl}1}^{\prime}(\vartheta_{m}) = \partial_{\vartheta_{m}}\ell_{\text{dubl}1}(\mathbf{P_{i}}, \mathbf{P_{m}},\vartheta_{i},\vartheta_{m})\ = \partial_{\beta}\ell_{\text{dubl}1}\partial_{\vartheta_{m}} \vartheta_{1}, \tag{48}\] \[\ell_{\text{dubl}2}^{\prime}(\vartheta_{m}) = \partial_{\vartheta_{m}}\ell_{\text{dubl}2}(\mathbf{P_{m}},\mathbf{ P_{f}},\vartheta_{m},\vartheta_{f})\ = \partial_{\alpha}\ell_{\text{dubl}2}\partial_{\vartheta_{m}} \alpha_{2}.\]  
Recalling that for a single segment \(\vartheta_{i}=\alpha-\phi\) and \(x_{f}=d\), \(y_{f}=0\), \(\vartheta_{f}=\beta-\phi\), and \(\phi=\text{atan2}(y_{f}-y_{i},x_{f}-x_{i})\), we obtain  
\[\partial_{\vartheta_{m}}\beta_{1} =\partial_{\vartheta_{m}}(\vartheta_{m}+\phi_{1})=1, \tag{49}\] \[\partial_{\vartheta_{m}}\alpha_{2} =\partial_{\vartheta_{m}}(\vartheta_{m}+\phi_{2})=1\]  
However, \(\phi_{1}=\text{atan2}(y_{m}-y_{i},x_{m}-x_{i})\) and \(\phi_{2}=\text{atan2}(y_{f}-y_{m},x_{f}-x_{m})\) do not depend on \(\vartheta_{m}\) and can be considered constants. Therefore, the derivatives of the lengths with respect to \(\vartheta_{m}\) are computed using (48) and (49):  
\[\ell_{\text{total}}^{\prime}(\vartheta_{m})=\partial_{\beta_{1}}\ell_{\text{ dubl}1}(\mathbf{P_{i}},\mathbf{P_{m}},\vartheta_{i},\beta_{1}-\phi_{1})+ \partial_{\alpha_{2}}\ell_{\text{dubl}2}(\mathbf{P_{m}},\mathbf{P_{f}},\alpha _{2}-\phi_{2},\vartheta_{f}).\]  
However, we cannot simply find the root of the previous equation because the lengths are piecewise continuous functions of \(\vartheta_{m}\). Therefore, we need to use a numerical method to find the optimal angle \(\vartheta_{m}\) that minimizes the total length of the path. Figure 2 illustrates the optimal configuration of the three-point Dubins connection. In the second subfigure, the total length is depicted as a function of the middle-point angle. This total length, which is the sum of the two individual lengths, can exhibit discontinuities. Depending on the problem's boundary conditions, some configurations are feasible only for a limited range of internal angles. Additionally, the third subplot displays the behavior of the analytical derivative of the total length. Notably, there are jumps in the derivative even in regions where the total length appears to be smooth. Therefore, finding the minimum length solution cannot rely on standard minimization or root-finding algorithms [19]. These traditional methods assume a certain level of smoothness and continuity in the target function and its derivatives.