void
StartPoint::find_best_start(const AircraftState &state,
const OrderedTaskPoint &next,
const TaskProjection &projection)
{
/* check which boundary point results in the smallest distance to
fly */
const OZBoundary boundary = next.GetBoundary();
assert(!boundary.empty());
const auto end = boundary.end();
auto i = boundary.begin();
assert(i != end);
const GeoPoint &next_location = next.GetLocationRemaining();
GeoPoint best_location = *i;
fixed best_distance = ::DoubleDistance(state.location, *i, next_location);
for (++i; i != end; ++i) {
fixed distance = ::DoubleDistance(state.location, *i, next_location);
if (distance < best_distance) {
best_location = *i;
best_distance = distance;
}
}
SetSearchMin(SearchPoint(best_location, projection));
}
void
SampledTaskPoint::SetSearchMin(const GeoPoint &location,
const TaskProjection &projection)
{
SearchPoint sp(location, projection);
SetSearchMin(sp);
}
void
StartPoint::find_best_start(const AircraftState &state,
const OrderedTaskPoint &next,
const TaskProjection &projection)
{
class StartPointBestStart: public ZeroFinder {
const StartPoint &start;
const GeoPoint loc_from;
const GeoPoint loc_to;
fixed p_offset;
public:
StartPointBestStart(const StartPoint& ts,
const GeoPoint &_loc_from,
const GeoPoint &_loc_to):
ZeroFinder(-fixed_half, fixed_half, fixed(0.01)),
start(ts),
loc_from(_loc_from),
loc_to(_loc_to) {};
virtual fixed f(const fixed p) {
return ::DoubleDistance(loc_from, parametric(p), loc_to);
}
GeoPoint solve() {
// find approx solution first, being the offset for the local function
// minimiser search
p_offset = fixed_zero;
fixed f_best= f(fixed_zero);
for (; p_offset < fixed_one; p_offset += fixed(0.25)) {
fixed ff = f(fixed_zero);
if (ff< f_best) {
f_best = ff;
}
}
// now detailed search, returning result
return parametric(find_min(fixed_zero));
}
private:
GeoPoint parametric(const fixed p) {
// ensure parametric input is between 0 and 1
fixed pp = p+p_offset;
if (negative(pp)) {
pp+= fixed_one;
}
pp = fmod(pp,fixed_one);
return start.GetBoundaryParametric(pp);
}
};
StartPointBestStart solver(*this, state.location,
next.GetLocationRemaining());
SetSearchMin(solver.solve(), projection);
}
