# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 6 Algorithms
### Pattern Search with Root Finder  
Algorithm 3 combines the potential of a pattern search method with a non-derivative root finder. The pattern search method is used iteratively to refine the interval of angles where the total length has a local minimum. The iterative search stops when the interval is small enough or whenever sufficient conditions to use the non-derivative root finder are met. The non-derivative root finder yields the optimal angle \(\theta_{m}\) inside the regular interval retrieved by the pattern search method. The sufficient conditions are that the solution does not change type inside the interval. If the derivatives at the interval's endpoints have opposite signs, then, by continuity, the minimum is inside the interval. In this case, a non-derivative root finder, which searches the zero of the derivative of the length, is used. The algorithm employed to find the root is a variant of the bisection with interpolation Algorithm 748 from [22]. ```
1:functionDubins3 pPatternRefine(P\({}_{i}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \(\theta_{m}\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\), \(\theta_{m}^{L}\), \(\theta_{m}^{C}\), \(\theta_{m}^{R}\))
2:\(L\leftarrow\textsc{TwoDubins}(\mathbf{P}_{\mathbf{i}}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \(\theta_{m}^{L}\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
3:\(C\leftarrow\textsc{TwoDubins}(\mathbf{P}_{\mathbf{i}}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \(\theta_{m}^{C}\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
4:\(R\leftarrow\textsc{TwoDubins}(\mathbf{P}_{\mathbf{i}}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \(\theta_{m}^{R}\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
5:while\(R.\theta_{m}-L.\theta_{m}>\Delta\rho_{\min}\)do (\(*\) refine solution \(*\))
6: OPTIONAL: If curve type for \(L\), \(C\) is the same
7: and the length derivative changes sign, use also 748
8: to refine the solution between \(L.\theta_{m}\) and \(C.\theta_{m}\) and return. 9:
10: OPTIONAL: If curve type for \(C\), \(R\) is the same
11: and the length derivative changes sign, use also 748
12: to refine the solution between \(C.\theta_{m}\) and \(R.\theta_{m}\) and return. 13:
14:\(\widehat{L}\leftarrow\textsc{TwoDubins}(\mathbf{P}_{\mathbf{i}}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \((L.\theta_{m}+C.\theta_{m})/2\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
15:\(\widehat{R}\leftarrow\textsc{TwoDubins}(\mathbf{P}_{\mathbf{i}}\), \(\theta_{i}\), \(\mathbf{P}_{\mathbf{m}}\), \((R.\theta_{m}+C.\theta_{m})/2\), \(\mathbf{P}_{\mathbf{f}}\), \(\theta_{f}\), \(\kappa_{max}\));
16:if\(\widehat{L}.\ell<\widehat{R}.\ell\)then
17:if\(\widehat{L}.\ell<C.\ell\)then
18:\(R\gets C\); \(C\leftarrow\widehat{L}\);
19:else
20:\(L\leftarrow\widehat{L}\); \(R\leftarrow\widehat{R}\);
21:endif
22:else
23:if\(\widehat{R}.\ell<C.\ell\)then
24:\(L\gets C\); \(C\leftarrow\widehat{R}\);
25:else
26:\(L\leftarrow\widehat{L}\); \(R\leftarrow\widehat{R}\);
27:endif
28:endif
29:endif
30:endwhile
31:return\(C\); (\(*\) best solution found \(*\))
32:endfunction
```  
**Algorithm 3** Pattern Search Refinement (Based on Direct Search [37])  
Algorithm 4 (Pattern Trichotomy) is similar to the previous one with a slight variation. The main drawback of the pattern search method is that it can find only one local minimum of the total length. Algorithm 4 overcomes, in part, this limitation using a pattern search method to find multiple local minima of the total length. The algorithm iteratively searches for the local minima of the total length and stores them in a list of candidate minima. Then, the algorithm explores the candidates in a neighborhood of decreasing size and stops the search in two cases. The first is when the interval is small enough. The second case is when sufficient conditions are met, where the fast bisection can be used on the total length derivative. In both cases, the solution found is stored if it is better than a previously computed one.