# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 6 Algorithms
### Pattern Search with Root Finder
The previously described methods (Algorithms 3 and 4) are not always able to catch the correct minima if there are discontinuities or jumps smaller than the first sampling range \(\Delta\theta\). Our proposed hybrid method (Algorithm 5) overcomes this problem by isolating first the discontinuity points, and then adding at least two more points (before and after the jump). Furthermore, additional points are sampled according to the interval range \(\Delta\theta\). The discontinuity isolation is performed by computing the existence range of the solution computed in (21), (34) and (35). The algorithm loops over all the sampled points, comparing the left and right samples. If the length at the central point is less or equal to the left and right lengths, we refine the solution. The algorithm returns the best solution found. The refinement phase is conducted by either the Dubins3 pPatternRefine or Dubins3 pTrichotomyRefine functions described in Algorithms 3 and 4. ```
1:functionDubins3 pTRichotomyRefine(\(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \(\theta_{m}\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\), \(\theta_{m}^{L}\), \(\theta_{m}^{C}\), \(\theta_{m}^{R}\) )
2:\(L\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \(\phi_{m}^{L}\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
3:\(C\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \(\theta_{m}^{R}\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
4:\(R\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \(\theta_{m}^{R}\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
5:while\(R.\theta_{m}-L.\theta_{m}>\Delta\theta_{min}\)do(\(\star\) refine solution \(\star\))
6: OPTIONAL: If curve type for \(L\), \(C\) is the same
7: and the length derivative change signs, use algo 748
8: to refine the solution between \(L.\theta_{m}\) and \(C.\theta_{m}\) and return. 9: OPTIONAL: If curve type for \(C\), \(R\) is the same
10: and the length derivative changes sign, use algo 748
11: to refine the solution between \(C.\theta_{m}\) and \(R.\theta_{m}\) and return. 12:\(\widehat{L}\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \((L.\theta_{m}+2C.\theta_{m})/3\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
13:if\(\widehat{L}.\ell\leq C.\ell\)then
14:\(C\leftarrow\widetilde{L}\); \(R\gets C\); \(C\leftarrow\widehat{L}\);
15:endif
16:else
17:else
18:\(\widehat{R}\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \((R.\theta_{m}+2C.\theta_{m})/3\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
19:if\(\widehat{R}.\ell\leq C.\ell\)then
20:\(\widehat{R}\leftarrow\textsc{TwoDubins}(\mathbf{P_{i}}\), \(\theta_{i}\), \(\mathbf{P_{m}}\), \((2R.\theta_{m}+C.\theta_{m})/3\), \(\mathbf{P_{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
21:if\(\widehat{R}.\ell\leq\widehat{R}.\ell\)then
22:\(L\leftarrow\widehat{R}\); \(C\leftarrow\widehat{R}\);
23:else
24:\(L\gets C\); \(C\leftarrow\widehat{R}\); \(R\leftarrow\widetilde{R}\);
25:endif
26:else
27:else
28:\(L\leftarrow\widehat{L}\); \(R\leftarrow\widehat{R}\);
29:endif
30:endif
31:endif
32:endif
33:endif
34:endif
35:endif
36:endif
37:endif
38:endif
39:



## 7 Numerical Tests and Results

In this section, we briefly present the numerical results and performance achievements. The tests were conducted on a commercial laptop with an Apple M2 Max processor. The test was performed on a sampled pool of \(10,000\) random three-point Dubins problems, which is the standard benchmark test for the 3 PDP found in the literature. The results are summarized in Tables 2-5. We employed two different ranges of sampling, the first coming from the literature and representing a standard benchmark case, whereas the second was more representative of the solution-type distribution. Without loss of generality, we kept the initial and final points fixed at \((-1,0)\) and \((1,0)\), respectively.