@@ -65,6 +65,55 @@ struct TGap
6565 }
6666};
6767
68+ // Returns the list of gaps found in `overall_scores` and the index of the
69+ // best (highest max_eval) gap, or npos if no gaps exist.
70+ static std::pair<std::vector<TGap>, std::size_t > findScoreGaps (
71+ const std::vector<double >& overall_scores)
72+ {
73+ const auto nDirs = overall_scores.size ();
74+ std::vector<TGap> gaps;
75+ std::size_t best_gap_idx = std::string::npos;
76+
77+ bool inside_gap = false ;
78+ for (unsigned int i = 0 ; i < nDirs; i++)
79+ {
80+ const double val = overall_scores[i];
81+ if (val < 0.01 )
82+ {
83+ if (inside_gap)
84+ {
85+ gaps.back ().k_to = i - 1 ;
86+ inside_gap = false ;
87+ }
88+ }
89+ else
90+ {
91+ if (!inside_gap)
92+ {
93+ TGap new_gap;
94+ new_gap.k_from = i;
95+ gaps.emplace_back (new_gap);
96+ inside_gap = true ;
97+ }
98+ }
99+
100+ if (inside_gap)
101+ {
102+ auto & active_gap = gaps.back ();
103+ if (val >= active_gap.max_eval ) active_gap.k_best_eval = i;
104+ mrpt::keep_max (active_gap.max_eval , val);
105+ mrpt::keep_min (active_gap.min_eval , val);
106+
107+ if (best_gap_idx == std::string::npos || val > gaps[best_gap_idx].max_eval )
108+ best_gap_idx = gaps.size () - 1 ;
109+ }
110+ }
111+
112+ if (inside_gap) gaps.back ().k_to = static_cast <int >(nDirs) - 1 ;
113+
114+ return {gaps, best_gap_idx};
115+ }
116+
68117void CHolonomicFullEval::evalSingleTarget (
69118 unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
70119{
@@ -428,69 +477,9 @@ CHolonomicFullEval::NavOutput CHolonomicFullEval::navigate(const NavInput& ni)
428477 for (unsigned int i = 0 ; i < nDirs; i++) overall_scores[i] *= (1.0 / numTrgs);
429478
430479 // Search for best direction in the "overall score" vector:
480+ // Keep the GAP with the largest maximum value; pick its best-eval direction.
481+ const auto [gaps, best_gap_idx] = findScoreGaps (overall_scores);
431482
432- // Keep the GAP with the largest maximum value within;
433- // then pick the MIDDLE point as the final selection.
434- std::vector<TGap> gaps;
435- std::size_t best_gap_idx = std::string::npos;
436- {
437- bool inside_gap = false ;
438- for (unsigned int i = 0 ; i < nDirs; i++)
439- {
440- const double val = overall_scores[i];
441- if (val < 0.01 )
442- {
443- // This direction didn't pass the cut threshold for the "last
444- // phase":
445- if (inside_gap)
446- {
447- // We just ended a gap:
448- auto & active_gap = *gaps.rbegin ();
449- active_gap.k_to = i - 1 ;
450- inside_gap = false ;
451- }
452- }
453- else
454- {
455- // higher or EQUAL to the treshold (equal is important just in
456- // case we have a "flat" diagram...)
457- if (!inside_gap)
458- {
459- // We just started a gap:
460- TGap new_gap;
461- new_gap.k_from = i;
462- gaps.emplace_back (new_gap);
463- inside_gap = true ;
464- }
465- }
466-
467- if (inside_gap)
468- {
469- auto & active_gap = *gaps.rbegin ();
470- if (val >= active_gap.max_eval )
471- {
472- active_gap.k_best_eval = i;
473- }
474- mrpt::keep_max (active_gap.max_eval , val);
475- mrpt::keep_min (active_gap.min_eval , val);
476-
477- if (best_gap_idx == std::string::npos || val > gaps[best_gap_idx].max_eval )
478- {
479- best_gap_idx = gaps.size () - 1 ;
480- }
481- }
482- } // end for i
483-
484- // Handle the case where we end with an open, active gap:
485- if (inside_gap)
486- {
487- auto & active_gap = *gaps.rbegin ();
488- active_gap.k_to = nDirs - 1 ;
489- }
490- }
491-
492- // Not heading to target: go thru the "middle" of the gap to maximize
493- // clearance
494483 int best_dir_k = -1 ;
495484 double best_dir_eval = 0 ;
496485
0 commit comments