use super::{CauchyPoint, Step, Subproblem, model_decrease, tau_to_boundary};
use crate::core::math::{
Dot, LinearSolveSpd, MatVec, NegInPlace, NormSquared, Scalar, ScaleInPlace, ScaledAdd,
};
#[derive(Debug, Clone, Copy, Default)]
pub struct Dogleg;
impl<V, M, F> Subproblem<V, M, F> for Dogleg
where
F: Scalar,
V: Clone + Dot<F> + NormSquared<F> + ScaledAdd<F> + ScaleInPlace<F> + NegInPlace,
M: MatVec<V> + LinearSolveSpd<V>,
{
fn solve(&self, g: &V, b: &M, radius: F) -> Step<V, F> {
let p_b = match b.solve_spd(g) {
Ok(mut p) => {
p.neg_in_place();
p
}
Err(_) => return CauchyPoint.solve(g, b, radius),
};
let p_b_norm = p_b.norm_squared().sqrt();
if p_b_norm <= radius {
let predicted_reduction = model_decrease(g, b, &p_b);
return Step {
d: p_b,
predicted_reduction,
hit_boundary: p_b_norm >= radius,
};
}
let bg = b.matvec(g);
let gbg = g.dot(&bg);
let gg = g.dot(g);
if gbg <= F::zero() {
let mut d = g.clone();
d.scale_in_place(-(radius / gg.sqrt()));
let predicted_reduction = model_decrease(g, b, &d);
return Step {
d,
predicted_reduction,
hit_boundary: true,
};
}
let mut p_u = g.clone();
p_u.scale_in_place(-(gg / gbg));
let p_u_norm = p_u.norm_squared().sqrt();
if p_u_norm >= radius {
let mut d = g.clone();
d.scale_in_place(-(radius / gg.sqrt()));
let predicted_reduction = model_decrease(g, b, &d);
return Step {
d,
predicted_reduction,
hit_boundary: true,
};
}
let mut diff = p_b;
diff.scaled_add(-F::one(), &p_u); let tau = tau_to_boundary(&p_u, &diff, radius);
let mut d = p_u;
d.scaled_add(tau, &diff);
let predicted_reduction = model_decrease(g, b, &d);
Step {
d,
predicted_reduction,
hit_boundary: true,
}
}
}