conspire/math/optimize/newton_raphson/
mod.rs

1#[cfg(test)]
2mod test;
3
4use super::{
5    super::{
6        Erase, Hessian, HessianBlock, Is, Jacobian, LuDecomposition, Matrix, Quantity, Scalar,
7        Solution, SquareMatrix, Tensor, Vector,
8        sparse::{CscMatrix, SparseSolver},
9    },
10    BacktrackingLineSearch, EqualityConstraint, FirstOrderRootFinding, FirstOrderRootFindingBlock,
11    FirstOrderRootFindingIncremental, LineSearch, LineSearchError, OptimizationError,
12    SecondOrderOptimization, SecondOrderOptimizationBlock, SecondOrderOptimizationIncremental,
13    SolveStrategy, Tolerances, TrustRegion,
14};
15use crate::math::Norm;
16use crate::units::{Dimensionless, UnitDiv, UnitMul, UnitSum};
17use std::{
18    fmt::{self, Debug, Formatter},
19    ops::{Div, Mul},
20};
21
22/// The Newton-Raphson method.
23#[derive(Clone)]
24pub struct NewtonRaphson {
25    /// Absolute error tolerances.
26    pub abs_tol: Tolerances,
27    /// Norm type for error evaluation.
28    pub error_norm: Norm,
29    /// Line search algorithm.
30    pub line_search: LineSearch,
31    /// Maximum number of steps.
32    pub max_steps: usize,
33    /// Relative error tolerance.
34    pub rel_tol: Option<Scalar>,
35    /// How far the step is trusted.
36    pub trust_region: TrustRegion,
37}
38
39impl<J, X> BacktrackingLineSearch<J, X> for NewtonRaphson {
40    fn get_line_search(&self) -> &LineSearch {
41        &self.line_search
42    }
43}
44
45impl Debug for NewtonRaphson {
46    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
47        write!(
48            f,
49            "NewtonRaphson {{ abs_tol: {:?}, line_search: {}, max_steps: {:?}, rel_tol: {:?}, trust_region: {:?} }}",
50            self.abs_tol, self.line_search, self.max_steps, self.rel_tol, self.trust_region
51        )
52    }
53}
54
55impl Default for NewtonRaphson {
56    fn default() -> Self {
57        Self {
58            abs_tol: Tolerances::default(),
59            error_norm: Norm::Chebyshev,
60            line_search: LineSearch::None,
61            max_steps: 25,
62            rel_tol: None,
63            trust_region: TrustRegion::None,
64        }
65    }
66}
67
68impl<F, J, X, E> FirstOrderRootFinding<F, J, X> for NewtonRaphson
69where
70    F: Jacobian,
71    for<'a> &'a F: Div<J, Output = X>,
72    J: Hessian,
73    F: Erase<Erased = E>,
74    X: Erase<Erased = E> + Solution,
75    E: Tensor,
76    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
77    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
78    for<'a> &'a Matrix: Mul<&'a X, Output = Vector>,
79{
80    fn root(
81        &self,
82        function: impl FnMut(&X) -> Result<F, String>,
83        jacobian: impl FnMut(&X) -> Result<J, String>,
84        initial_guess: X,
85        equality_constraint: EqualityConstraint,
86        sparse: Option<SparseSolver>,
87    ) -> Result<X, OptimizationError> {
88        match equality_constraint {
89            EqualityConstraint::Fixed(indices) => constrained_fixed(
90                self,
91                |_: &X| panic!("No line search in root finding"),
92                function,
93                jacobian,
94                |_: &X, _: &Vector, _: Scalar, _: bool| Ok(()),
95                initial_guess,
96                sparse,
97                indices,
98            ),
99            EqualityConstraint::Linear(constraint_matrix, constraint_rhs) => constrained(
100                self,
101                |_: &X| panic!("No line search in root finding"),
102                function,
103                jacobian,
104                |_: &X, _: &Vector, _: Scalar, _: bool| Ok(()),
105                initial_guess,
106                sparse,
107                constraint_matrix,
108                constraint_rhs,
109            ),
110            EqualityConstraint::None => unconstrained(
111                self,
112                |_: &X| panic!("No line search in root finding"),
113                function,
114                jacobian,
115                initial_guess,
116                sparse,
117            ),
118        }
119        .map_err(|error| OptimizationError::upstream(error, self))
120    }
121}
122
123impl<F, J, X, E> FirstOrderRootFindingIncremental<F, J, X> for NewtonRaphson
124where
125    F: Jacobian,
126    for<'a> &'a F: Div<J, Output = X>,
127    J: Hessian,
128    F: Erase<Erased = E>,
129    X: Erase<Erased = E> + Solution,
130    E: Tensor,
131    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
132    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
133    for<'a> &'a Matrix: Mul<&'a X, Output = Vector>,
134{
135    fn root_incremental(
136        &self,
137        function: impl FnMut(&X) -> Result<F, String>,
138        jacobian: impl FnMut(&X) -> Result<J, String>,
139        update: impl FnMut(&X, &Vector, Scalar, bool) -> Result<(), String>,
140        initial_guess: X,
141        equality_constraint: EqualityConstraint,
142        sparse: Option<SparseSolver>,
143    ) -> Result<X, OptimizationError> {
144        match equality_constraint {
145            EqualityConstraint::Fixed(indices) => constrained_fixed(
146                self,
147                |_: &X| panic!("No line search in root finding"),
148                function,
149                jacobian,
150                update,
151                initial_guess,
152                sparse,
153                indices,
154            ),
155            EqualityConstraint::Linear(constraint_matrix, constraint_rhs) => constrained(
156                self,
157                |_: &X| panic!("No line search in root finding"),
158                function,
159                jacobian,
160                update,
161                initial_guess,
162                sparse,
163                constraint_matrix,
164                constraint_rhs,
165            ),
166            EqualityConstraint::None => unimplemented!(
167                "An unconstrained solution has no chained vector to lend the increment through."
168            ),
169        }
170        .map_err(|error| OptimizationError::upstream(error, self))
171    }
172}
173
174impl<F, J, H, X, E> SecondOrderOptimization<F, J, H, X> for NewtonRaphson
175where
176    F: Erase<Erased = Scalar> + Tensor,
177    <J as Tensor>::Unit: UnitMul<<X as Tensor>::Unit>,
178    <<J as Tensor>::Unit as UnitMul<<X as Tensor>::Unit>>::Output: UnitSum,
179    <<<J as Tensor>::Unit as UnitMul<<X as Tensor>::Unit>>::Output as UnitSum>::Output:
180        Is<<F as Tensor>::Unit>,
181    H: Hessian,
182    J: Jacobian,
183    for<'a> &'a J: Div<H, Output = X>,
184    J: Erase<Erased = E>,
185    X: Erase<Erased = E> + Solution,
186    E: Tensor,
187    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
188    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
189    for<'a> &'a Matrix: Mul<&'a X, Output = Vector>,
190{
191    fn minimize(
192        &self,
193        mut function: impl FnMut(&X) -> Result<F, String>,
194        jacobian: impl FnMut(&X) -> Result<J, String>,
195        hessian: impl FnMut(&X) -> Result<H, String>,
196        initial_guess: X,
197        equality_constraint: EqualityConstraint,
198        sparse: Option<SparseSolver>,
199    ) -> Result<X, OptimizationError> {
200        let function = move |argument: &X| function(argument).map(|value| *value.erase());
201        match equality_constraint {
202            EqualityConstraint::Fixed(indices) => constrained_fixed(
203                self,
204                function,
205                jacobian,
206                hessian,
207                |_: &X, _: &Vector, _: Scalar, _: bool| Ok(()),
208                initial_guess,
209                sparse,
210                indices,
211            ),
212            EqualityConstraint::Linear(constraint_matrix, constraint_rhs) => constrained(
213                self,
214                function,
215                jacobian,
216                hessian,
217                |_: &X, _: &Vector, _: Scalar, _: bool| Ok(()),
218                initial_guess,
219                sparse,
220                constraint_matrix,
221                constraint_rhs,
222            ),
223            EqualityConstraint::None => {
224                unconstrained(self, function, jacobian, hessian, initial_guess, sparse)
225            }
226        }
227        .map_err(|error| OptimizationError::upstream(error, self))
228    }
229}
230
231impl<F, J, H, X, E> SecondOrderOptimizationIncremental<F, J, H, X> for NewtonRaphson
232where
233    F: Erase<Erased = Scalar> + Tensor,
234    <J as Tensor>::Unit: UnitMul<<X as Tensor>::Unit>,
235    <<J as Tensor>::Unit as UnitMul<<X as Tensor>::Unit>>::Output: UnitSum,
236    <<<J as Tensor>::Unit as UnitMul<<X as Tensor>::Unit>>::Output as UnitSum>::Output:
237        Is<<F as Tensor>::Unit>,
238    H: Hessian,
239    J: Jacobian,
240    for<'a> &'a J: Div<H, Output = X>,
241    J: Erase<Erased = E>,
242    X: Erase<Erased = E> + Solution,
243    E: Tensor,
244    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
245    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
246    for<'a> &'a Matrix: Mul<&'a X, Output = Vector>,
247{
248    fn minimize_incremental(
249        &self,
250        mut function: impl FnMut(&X) -> Result<F, String>,
251        jacobian: impl FnMut(&X) -> Result<J, String>,
252        hessian: impl FnMut(&X) -> Result<H, String>,
253        update: impl FnMut(&X, &Vector, Scalar, bool) -> Result<(), String>,
254        initial_guess: X,
255        equality_constraint: EqualityConstraint,
256        sparse: Option<SparseSolver>,
257    ) -> Result<X, OptimizationError> {
258        let function = move |argument: &X| function(argument).map(|value| *value.erase());
259        match equality_constraint {
260            EqualityConstraint::Fixed(indices) => constrained_fixed(
261                self,
262                function,
263                jacobian,
264                hessian,
265                update,
266                initial_guess,
267                sparse,
268                indices,
269            ),
270            EqualityConstraint::Linear(constraint_matrix, constraint_rhs) => constrained(
271                self,
272                function,
273                jacobian,
274                hessian,
275                update,
276                initial_guess,
277                sparse,
278                constraint_matrix,
279                constraint_rhs,
280            ),
281            EqualityConstraint::None => unimplemented!(
282                "An unconstrained solution has no chained vector to lend the increment through."
283            ),
284        }
285        .map_err(|error| OptimizationError::upstream(error, self))
286    }
287}
288
289impl<U, V, Ru, Rv, Kuu, Kvu, Kuv, Kvv> FirstOrderRootFindingBlock<U, V, Ru, Rv, Kuu, Kvu, Kuv, Kvv>
290    for NewtonRaphson
291where
292    U: Solution,
293    V: Solution,
294    Ru: Jacobian,
295    Rv: Jacobian,
296    Kuu: HessianBlock,
297    Kvu: HessianBlock,
298    Kuv: HessianBlock,
299    Kvv: HessianBlock,
300    for<'a> &'a CscMatrix: Mul<&'a U, Output = Vector> + Mul<&'a V, Output = Vector>,
301{
302    fn root_block(
303        &self,
304        residual_global: impl FnMut(&U, &V) -> Result<Ru, String>,
305        residual_local: impl FnMut(&U, &V) -> Result<Rv, String>,
306        tangents: impl FnMut(&U, &V) -> Result<(Kuu, Kvu, Kuv, Kvv), String>,
307        initial_guess: (U, V),
308        constraint_global: (CscMatrix, Vector),
309        constraint_local: (CscMatrix, Vector),
310        sparse: Option<SparseSolver>,
311        strategy: SolveStrategy,
312    ) -> Result<(U, V), OptimizationError> {
313        blocked(
314            self,
315            |_: &U, _: &V| panic!("No line search in root finding"),
316            false,
317            residual_global,
318            residual_local,
319            tangents,
320            initial_guess,
321            constraint_global,
322            constraint_local,
323            sparse,
324            strategy,
325        )
326        .map_err(|error| OptimizationError::upstream(error, self))
327    }
328}
329
330impl<F, U, V, Ru, Rv, Kuu, Kvu, Kuv, Kvv>
331    SecondOrderOptimizationBlock<F, U, V, Ru, Rv, Kuu, Kvu, Kuv, Kvv> for NewtonRaphson
332where
333    F: Erase<Erased = Scalar> + Tensor,
334    <Ru as Tensor>::Unit: UnitMul<<U as Tensor>::Unit>,
335    <<Ru as Tensor>::Unit as UnitMul<<U as Tensor>::Unit>>::Output: UnitSum,
336    <<<Ru as Tensor>::Unit as UnitMul<<U as Tensor>::Unit>>::Output as UnitSum>::Output:
337        Is<<F as Tensor>::Unit>,
338    <Rv as Tensor>::Unit: UnitMul<<V as Tensor>::Unit>,
339    <<Rv as Tensor>::Unit as UnitMul<<V as Tensor>::Unit>>::Output: UnitSum,
340    <<<Rv as Tensor>::Unit as UnitMul<<V as Tensor>::Unit>>::Output as UnitSum>::Output:
341        Is<<F as Tensor>::Unit>,
342    U: Solution,
343    V: Solution,
344    Ru: Jacobian,
345    Rv: Jacobian,
346    Kuu: HessianBlock,
347    Kvu: HessianBlock,
348    Kuv: HessianBlock,
349    Kvv: HessianBlock,
350    for<'a> &'a CscMatrix: Mul<&'a U, Output = Vector> + Mul<&'a V, Output = Vector>,
351{
352    fn minimize_block(
353        &self,
354        mut function: impl FnMut(&U, &V) -> Result<F, String>,
355        residual_global: impl FnMut(&U, &V) -> Result<Ru, String>,
356        residual_local: impl FnMut(&U, &V) -> Result<Rv, String>,
357        tangents: impl FnMut(&U, &V) -> Result<(Kuu, Kvu, Kuv, Kvv), String>,
358        initial_guess: (U, V),
359        constraint_global: (CscMatrix, Vector),
360        constraint_local: (CscMatrix, Vector),
361        sparse: Option<SparseSolver>,
362        strategy: SolveStrategy,
363    ) -> Result<(U, V), OptimizationError> {
364        let function = move |global: &U, local: &V| function(global, local).map(|v| *v.erase());
365        blocked(
366            self,
367            function,
368            true,
369            residual_global,
370            residual_local,
371            tangents,
372            initial_guess,
373            constraint_global,
374            constraint_local,
375            sparse,
376            strategy,
377        )
378        .map_err(|error| OptimizationError::upstream(error, self))
379    }
380}
381
382const PENALTY_SAFETY: Scalar = 2.0;
383
384fn violation<M, T>(constraint_matrix: &M, constraint_rhs: &Vector, variables: &T) -> Scalar
385where
386    for<'a> &'a M: Mul<&'a T, Output = Vector>,
387{
388    (constraint_rhs - constraint_matrix * variables)
389        .iter()
390        .map(|entry| entry.abs())
391        .sum()
392}
393
394/// Raises the penalty until it outweighs every multiplier the step would reach.
395///
396/// The penalty only ever climbs, so a step taken where the multipliers were
397/// larger is not undone by one taken where they are smaller.
398fn raise_penalty<'a>(
399    penalty: Scalar,
400    multipliers: impl Iterator<Item = (&'a Scalar, &'a Scalar)>,
401) -> Scalar {
402    penalty.max(
403        PENALTY_SAFETY
404            * multipliers.fold(0.0, |largest: Scalar, (multiplier, decrement)| {
405                largest.max((multiplier - decrement).abs())
406            }),
407    )
408}
409
410/// The slope of the merit function along the step.
411///
412/// Stated in the sign the line search reads, where a positive slope is the
413/// decrease a step buys, so the violation the penalty charges for adds to the
414/// descent the gradient promises rather than subtracting from it.
415fn merit_slope<'a>(
416    gradients: impl Iterator<Item = (&'a Scalar, &'a Scalar)>,
417    violated: Scalar,
418) -> Scalar {
419    gradients
420        .map(|(gradient, decrement)| gradient * decrement)
421        .sum::<Scalar>()
422        + violated
423}
424
425/// Backtracks on the merit function, or takes the whole step where there is
426/// nothing to backtrack along.
427///
428/// The line search refuses a direction that is not one of descent, and cannot
429/// resolve one whose descent is finer than the merit it would be measured
430/// against: the conditions all compare a decrease that the subtraction has
431/// already rounded away. Either is met by stepping whole rather than by
432/// asking and being turned away.
433///
434/// A slope is a merit against a step, so it is the merit it is judged
435/// against, which leaves the comparison a ratio and the threshold the
436/// precision that ratio is held in. A number of its own would carry units and
437/// mean something different at every scale.
438fn backtrack_penalty(
439    newton_raphson: &NewtonRaphson,
440    merit: impl FnMut(Scalar) -> Result<Scalar, String>,
441    value: Scalar,
442    slope: Scalar,
443) -> Result<Scalar, OptimizationError> {
444    if slope <= Scalar::EPSILON * value.abs() {
445        Ok(1.0)
446    } else {
447        newton_raphson
448            .line_search
449            .backtrack_merit(merit, value, slope, 1.0)
450            .map_err(|error| OptimizationError::upstream(error, newton_raphson))
451    }
452}
453
454#[expect(clippy::too_many_arguments)]
455fn kkt_entry<Kuu, Kvu, Kuv, Kvv>(
456    row: usize,
457    column: usize,
458    num_global: usize,
459    num_outer: usize,
460    num_local: usize,
461    tangent_uu: &Kuu,
462    tangent_vu: &Kvu,
463    tangent_uv: &Kuv,
464    tangent_vv: &Kvv,
465    constraint_matrix_global: &CscMatrix,
466    constraint_matrix_local: &CscMatrix,
467) -> Scalar
468where
469    Kuu: HessianBlock,
470    Kvu: HessianBlock,
471    Kuv: HessianBlock,
472    Kvv: HessianBlock,
473{
474    let local = num_outer + num_local;
475    let row_global = row < num_global;
476    let row_local = (num_outer..local).contains(&row);
477    let column_global = column < num_global;
478    let column_local = (num_outer..local).contains(&column);
479    if row_global && column_global {
480        tangent_uu.entry(row, column)
481    } else if row_global && column_local {
482        tangent_uv.entry(row, column - num_outer)
483    } else if row_local && column_global {
484        tangent_vu.entry(row - num_outer, column)
485    } else if row_local && column_local {
486        tangent_vv.entry(row - num_outer, column - num_outer)
487    } else if row_global && (num_global..num_outer).contains(&column) {
488        -constraint_matrix_global.entry(column - num_global, row)
489    } else if (num_global..num_outer).contains(&row) && column_global {
490        -constraint_matrix_global.entry(row - num_global, column)
491    } else if row_local && column >= local {
492        -constraint_matrix_local.entry(column - local, row - num_outer)
493    } else if row >= local && column_local {
494        -constraint_matrix_local.entry(row - local, column - num_outer)
495    } else {
496        0.0
497    }
498}
499
500/// Shortens the step until it lands somewhere the problem can be evaluated,
501/// or gives up.
502///
503/// This asks nothing of a merit function, so it is the one line search root
504/// finding can also take, and it says nothing about descent. Where a trial
505/// point is, and what makes it reachable, is left to the formulation asking:
506/// whatever was eliminated is stepped alongside, so the state to test is the
507/// one everything arrives at together.
508fn backtrack_errors(
509    newton_raphson: &NewtonRaphson,
510    mut reachable: impl FnMut(Scalar) -> bool,
511    cut_back: Scalar,
512    max_steps: usize,
513) -> Result<Scalar, OptimizationError> {
514    let mut trial_size = 1.0;
515    for _ in 0..max_steps {
516        if reachable(trial_size) {
517            return Ok(trial_size);
518        }
519        trial_size *= cut_back
520    }
521    Err(OptimizationError::upstream(
522        LineSearchError::MaximumStepsReached(
523            format!("{:?}", newton_raphson.line_search),
524            max_steps,
525        ),
526        newton_raphson,
527    ))
528}
529
530/// Whether every block of the residual has come within the tolerance.
531///
532/// Each block is measured on its own against a tolerance of its own, the
533/// constraint violation being of another kind entirely from the residual, and
534/// either kind in one block from that in another.
535///
536/// The scales are what each block was on the first step, so that the relative
537/// tolerance is compared against a ratio of two norms of the same kind, and
538/// means the same thing whatever units that kind is measured in.
539fn converged(
540    newton_raphson: &NewtonRaphson,
541    residual: &Vector,
542    variables: usize,
543    scales: &mut Option<(Scalar, Scalar)>,
544) -> bool {
545    let norms = (
546        newton_raphson
547            .error_norm
548            .over(residual.iter().take(variables).copied()),
549        newton_raphson
550            .error_norm
551            .over(residual.iter().skip(variables).copied()),
552    );
553    let scales = scales.get_or_insert(norms);
554    let met = |norm: Scalar, scale: Scalar, abs_tol: Scalar| {
555        norm < abs_tol
556            || newton_raphson
557                .rel_tol
558                .is_some_and(|rel_tol| norm / scale < rel_tol)
559    };
560    met(norms.0, scales.0, newton_raphson.abs_tol.residual)
561        && met(norms.1, scales.1, newton_raphson.abs_tol.constraint)
562}
563
564/// Shortens the step until the variables move no further than the maximum.
565///
566/// Only the variables are measured, the multipliers being of another kind
567/// entirely, but everything is scaled together so that the direction survives.
568fn limit_decrement(newton_raphson: &NewtonRaphson, decrements: &mut [(&mut Vector, usize)]) {
569    if let TrustRegion::Fixed { radius, norm } = newton_raphson.trust_region {
570        let size = norm.over(
571            decrements
572                .iter()
573                .flat_map(|(decrement, variables)| decrement.iter().take(*variables).copied()),
574        );
575        if size > radius {
576            decrements
577                .iter_mut()
578                .for_each(|(decrement, _)| **decrement *= radius / size)
579        }
580    }
581}
582
583fn kkt_block<K>(
584    tangent: &K,
585    constraint_matrix: &CscMatrix,
586    size: usize,
587    block: &mut SquareMatrix,
588    offset: usize,
589) where
590    K: HessianBlock,
591{
592    tangent.fill_into_block(block, offset, offset);
593    constraint_matrix.iter().for_each(|(a, j, entry)| {
594        block[offset + size + a][offset + j] = -entry;
595        block[offset + j][offset + size + a] = -entry;
596    })
597}
598
599fn kkt_residual<R, T>(
600    residual: R,
601    multipliers: &Vector,
602    constraint_matrix: &CscMatrix,
603    constraint_rhs: &Vector,
604    variables: &T,
605    chained: &mut Vector,
606) where
607    R: Jacobian,
608    for<'a> &'a CscMatrix: Mul<&'a T, Output = Vector>,
609{
610    (residual - multipliers * constraint_matrix)
611        .fill_into_chained(constraint_rhs - constraint_matrix * variables, chained)
612}
613
614/// Converges the local variables at fixed global ones.
615///
616/// The condensed strategy treats the local variables as a function of the
617/// global ones, so anywhere the global variables are moved to, this is what
618/// the local ones become.
619#[expect(clippy::too_many_arguments)]
620fn converge_local<U, V, Rv, Kuu, Kvu, Kuv, Kvv>(
621    local_solver: &NewtonRaphson,
622    residual_local: &mut impl FnMut(&U, &V) -> Result<Rv, String>,
623    tangents: &mut impl FnMut(&U, &V) -> Result<(Kuu, Kvu, Kuv, Kvv), String>,
624    global: &U,
625    local: &mut V,
626    multipliers_local: &mut Vector,
627    constraint_matrix_local: &CscMatrix,
628    constraint_rhs_local: &Vector,
629    num_local: usize,
630    update_inner: &mut Vector,
631    tangent_inner: &mut SquareMatrix,
632    factorization: &mut LuDecomposition,
633) -> Result<(), OptimizationError>
634where
635    Rv: Jacobian,
636    Kvv: HessianBlock,
637    V: Solution,
638    for<'a> &'a CscMatrix: Mul<&'a V, Output = Vector>,
639{
640    let mut local_steps = 0;
641    let mut scales = None;
642    loop {
643        kkt_residual(
644            residual_local(global, local)?,
645            multipliers_local,
646            constraint_matrix_local,
647            constraint_rhs_local,
648            local,
649            update_inner,
650        );
651        if converged(local_solver, update_inner, num_local, &mut scales)
652            || local_steps == local_solver.max_steps
653        {
654            return Ok(());
655        }
656        local_steps += 1;
657        let (_, _, _, tangent) = tangents(global, local)?;
658        kkt_block(
659            &tangent,
660            constraint_matrix_local,
661            num_local,
662            tangent_inner,
663            0,
664        );
665        tangent_inner.factorize_lu_into(factorization)?;
666        let mut decrement = factorization.solve(update_inner);
667        limit_decrement(local_solver, &mut [(&mut decrement, num_local)]);
668        local.decrement_from_chained(multipliers_local, &decrement)
669    }
670}
671
672#[expect(clippy::too_many_arguments)]
673fn blocked<U, V, Ru, Rv, Kuu, Kvu, Kuv, Kvv>(
674    newton_raphson: &NewtonRaphson,
675    mut function: impl FnMut(&U, &V) -> Result<Scalar, String>,
676    minimizing: bool,
677    mut residual_global: impl FnMut(&U, &V) -> Result<Ru, String>,
678    mut residual_local: impl FnMut(&U, &V) -> Result<Rv, String>,
679    mut tangents: impl FnMut(&U, &V) -> Result<(Kuu, Kvu, Kuv, Kvv), String>,
680    initial_guess: (U, V),
681    constraint_global: (CscMatrix, Vector),
682    constraint_local: (CscMatrix, Vector),
683    sparse: Option<SparseSolver>,
684    strategy: SolveStrategy,
685) -> Result<(U, V), OptimizationError>
686where
687    U: Solution,
688    V: Solution,
689    Ru: Jacobian,
690    Rv: Jacobian,
691    Kuu: HessianBlock,
692    Kvu: HessianBlock,
693    Kuv: HessianBlock,
694    Kvv: HessianBlock,
695    for<'a> &'a CscMatrix: Mul<&'a U, Output = Vector> + Mul<&'a V, Output = Vector>,
696{
697    let (mut global, mut local) = initial_guess;
698    let mut penalty = 0.0 as Scalar;
699    let (constraint_matrix_global, constraint_rhs_global) = constraint_global;
700    let (constraint_matrix_local, constraint_rhs_local) = constraint_local;
701    let num_global = global.size();
702    let num_local = local.size();
703    let num_outer = num_global + constraint_rhs_global.len();
704    let num_inner = num_local + constraint_rhs_local.len();
705    let mut multipliers_global = Vector::zero(constraint_rhs_global.len());
706    let mut multipliers_local = Vector::zero(constraint_rhs_local.len());
707    let eliminating = !matches!(strategy, SolveStrategy::Monolithic { elimination: false });
708    let condensed = match strategy {
709        SolveStrategy::Condensed(ref local_solver) => Some(local_solver),
710        SolveStrategy::Monolithic { .. } => None,
711    };
712    if sparse.is_some() && eliminating {
713        unimplemented!(
714            "Eliminating the local block sparsely wants it held as the blocks it is, not as one matrix."
715        )
716    }
717    let (inner, outer) = if eliminating {
718        (num_inner, num_outer)
719    } else {
720        (0, 0)
721    };
722    let whole = if eliminating || sparse.is_some() {
723        0
724    } else {
725        num_outer + num_inner
726    };
727    let mut column = Vector::zero(inner);
728    let mut coupling_global = Matrix::zero(outer, inner);
729    let mut coupling_local = Matrix::zero(inner, outer);
730    let mut decrement_inner = Vector::zero(num_inner);
731    let mut decrement_outer = Vector::zero(num_outer);
732    let mut decrement_whole = Vector::zero(whole);
733    let mut eliminated = vec![Vector::zero(inner); outer.min(num_global)];
734    let mut factorization = LuDecomposition::zero(inner);
735    let mut factorization_outer = LuDecomposition::zero(outer);
736    let mut factorization_whole = LuDecomposition::zero(whole);
737    let mut monolithic = SquareMatrix::zero(whole);
738    let mut residual = Vector::zero(num_outer + num_inner);
739    let mut scales_inner = None;
740    let mut scales_outer = None;
741    let mut tangent_inner = SquareMatrix::zero(inner);
742    let mut tangent_outer = SquareMatrix::zero(outer);
743    let mut update_inner = Vector::zero(num_inner);
744    let mut update_outer = Vector::zero(num_outer);
745    let mut steps = 0;
746    loop {
747        if let Some(local_solver) = condensed {
748            converge_local(
749                local_solver,
750                &mut residual_local,
751                &mut tangents,
752                &global,
753                &mut local,
754                &mut multipliers_local,
755                &constraint_matrix_local,
756                &constraint_rhs_local,
757                num_local,
758                &mut update_inner,
759                &mut tangent_inner,
760                &mut factorization,
761            )?
762        }
763        kkt_residual(
764            residual_global(&global, &local)?,
765            &multipliers_global,
766            &constraint_matrix_global,
767            &constraint_rhs_global,
768            &global,
769            &mut update_outer,
770        );
771        kkt_residual(
772            residual_local(&global, &local)?,
773            &multipliers_local,
774            &constraint_matrix_local,
775            &constraint_rhs_local,
776            &local,
777            &mut update_inner,
778        );
779        update_outer
780            .iter()
781            .chain(update_inner.iter())
782            .zip(residual.iter_mut())
783            .for_each(|(entry, residual_i)| *residual_i = *entry);
784        let converged_outer =
785            converged(newton_raphson, &update_outer, num_global, &mut scales_outer);
786        let converged_inner =
787            converged(newton_raphson, &update_inner, num_local, &mut scales_inner);
788        if converged_outer && converged_inner {
789            return Ok((global, local));
790        } else if steps == newton_raphson.max_steps {
791            return Err(OptimizationError::MaximumStepsReached(
792                newton_raphson.max_steps,
793                format!("{newton_raphson:?}"),
794            ));
795        }
796        steps += 1;
797        let (tangent_uu, tangent_vu, tangent_uv, tangent_vv) = tangents(&global, &local)?;
798        if eliminating {
799            kkt_block(
800                &tangent_uu,
801                &constraint_matrix_global,
802                num_global,
803                &mut tangent_outer,
804                0,
805            );
806            kkt_block(
807                &tangent_vv,
808                &constraint_matrix_local,
809                num_local,
810                &mut tangent_inner,
811                0,
812            );
813            tangent_uv.fill_into_block(&mut coupling_global, 0, 0);
814            tangent_vu.fill_into_block(&mut coupling_local, 0, 0);
815            tangent_inner.factorize_lu_into(&mut factorization)?;
816            eliminated
817                .iter_mut()
818                .enumerate()
819                .for_each(|(k, eliminated_k)| {
820                    (0..num_local).for_each(|i| column[i] = coupling_local[i][k]);
821                    factorization.solve_into(&column, eliminated_k)
822                });
823            factorization.solve_into(&update_inner, &mut decrement_inner);
824            (0..num_global).for_each(|i| {
825                (0..num_local).for_each(|j| {
826                    let coupling = coupling_global[i][j];
827                    (0..num_global)
828                        .for_each(|k| tangent_outer[i][k] -= coupling * eliminated[k][j]);
829                    update_outer[i] -= coupling * decrement_inner[j]
830                })
831            });
832            tangent_outer.factorize_lu_into(&mut factorization_outer)?;
833            factorization_outer.solve_into(&update_outer, &mut decrement_outer);
834            (0..num_global).for_each(|k| {
835                decrement_inner
836                    .iter_mut()
837                    .zip(eliminated[k].iter())
838                    .for_each(|(decrement_inner_i, eliminated_ki)| {
839                        *decrement_inner_i -= eliminated_ki * decrement_outer[k]
840                    })
841            });
842        } else {
843            if sparse.is_none() {
844                kkt_block(
845                    &tangent_uu,
846                    &constraint_matrix_global,
847                    num_global,
848                    &mut monolithic,
849                    0,
850                );
851                kkt_block(
852                    &tangent_vv,
853                    &constraint_matrix_local,
854                    num_local,
855                    &mut monolithic,
856                    num_outer,
857                );
858            }
859            if let Some(ref solver) = sparse {
860                //
861                // The block layout is the same either way, so the entry a
862                // sparse solver asks for is read from whichever block holds it.
863                //
864                decrement_whole = solver.solve(
865                    |i, j| {
866                        kkt_entry(
867                            i,
868                            j,
869                            num_global,
870                            num_outer,
871                            num_local,
872                            &tangent_uu,
873                            &tangent_vu,
874                            &tangent_uv,
875                            &tangent_vv,
876                            &constraint_matrix_global,
877                            &constraint_matrix_local,
878                        )
879                    },
880                    &residual,
881                )?
882            } else {
883                tangent_uv.fill_into_block(&mut monolithic, 0, num_outer);
884                tangent_vu.fill_into_block(&mut monolithic, num_outer, 0);
885                monolithic.factorize_lu_into(&mut factorization_whole)?;
886                factorization_whole.solve_into(&residual, &mut decrement_whole)
887            }
888            decrement_whole
889                .iter()
890                .zip(decrement_outer.iter_mut().chain(decrement_inner.iter_mut()))
891                .for_each(|(entry, decrement_i)| *decrement_i = *entry);
892        }
893        limit_decrement(
894            newton_raphson,
895            &mut [
896                (&mut decrement_outer, num_global),
897                (&mut decrement_inner, num_local),
898            ],
899        );
900        let step_size = if matches!(newton_raphson.line_search, LineSearch::None) {
901            1.0
902        } else if !minimizing
903            && let LineSearch::Error {
904                cut_back,
905                max_steps,
906            } = &newton_raphson.line_search
907        {
908            //
909            // Root finding has no merit function to backtrack against, so the
910            // trial point is judged by whether the problem can be evaluated
911            // there at all. Minimization keeps its merit function instead.
912            //
913            backtrack_errors(
914                newton_raphson,
915                |trial_size| {
916                    let mut trial_global = global.clone();
917                    let mut trial_local = local.clone();
918                    let mut trial_multipliers_global = multipliers_global.clone();
919                    let mut trial_multipliers_local = multipliers_local.clone();
920                    trial_global.decrement_from_chained(
921                        &mut trial_multipliers_global,
922                        &(&decrement_outer * trial_size),
923                    );
924                    let reached = if let Some(local_solver) = condensed {
925                        converge_local(
926                            local_solver,
927                            &mut residual_local,
928                            &mut tangents,
929                            &trial_global,
930                            &mut trial_local,
931                            &mut trial_multipliers_local,
932                            &constraint_matrix_local,
933                            &constraint_rhs_local,
934                            num_local,
935                            &mut update_inner,
936                            &mut tangent_inner,
937                            &mut factorization,
938                        )
939                        .is_ok()
940                    } else {
941                        trial_local.decrement_from_chained(
942                            &mut trial_multipliers_local,
943                            &(&decrement_inner * trial_size),
944                        );
945                        true
946                    };
947                    reached
948                        && residual_global(&trial_global, &trial_local).is_ok()
949                        && residual_local(&trial_global, &trial_local).is_ok()
950                        && tangents(&trial_global, &trial_local).is_ok()
951                },
952                *cut_back,
953                *max_steps,
954            )?
955        } else {
956            penalty = raise_penalty(
957                penalty,
958                multipliers_global
959                    .iter()
960                    .zip(decrement_outer.iter().skip(num_global))
961                    .chain(
962                        multipliers_local
963                            .iter()
964                            .zip(decrement_inner.iter().skip(num_local)),
965                    ),
966            );
967            let violated = penalty
968                * (violation(&constraint_matrix_global, &constraint_rhs_global, &global)
969                    + violation(&constraint_matrix_local, &constraint_rhs_local, &local));
970            let mut gradient_global = Vector::zero(num_global);
971            let mut gradient_local = Vector::zero(num_local);
972            residual_global(&global, &local)?.fill_into(&mut gradient_global);
973            residual_local(&global, &local)?.fill_into(&mut gradient_local);
974            let slope = merit_slope(
975                gradient_global
976                    .iter()
977                    .zip(decrement_outer.iter())
978                    .chain(gradient_local.iter().zip(decrement_inner.iter())),
979                violated,
980            );
981            let value = function(&global, &local)? + violated;
982            backtrack_penalty(
983                newton_raphson,
984                |step| {
985                    let mut trial_global = global.clone();
986                    let mut trial_local = local.clone();
987                    let mut trial_multipliers_global = multipliers_global.clone();
988                    let mut trial_multipliers_local = multipliers_local.clone();
989                    trial_global.decrement_from_chained(
990                        &mut trial_multipliers_global,
991                        &(&decrement_outer * step),
992                    );
993                    //
994                    // Condensed makes the local variables a function of the
995                    // global ones, so a trial point is where they solve to,
996                    // not where the increment predicted they would.
997                    //
998                    if let Some(local_solver) = condensed {
999                        converge_local(
1000                            local_solver,
1001                            &mut residual_local,
1002                            &mut tangents,
1003                            &trial_global,
1004                            &mut trial_local,
1005                            &mut trial_multipliers_local,
1006                            &constraint_matrix_local,
1007                            &constraint_rhs_local,
1008                            num_local,
1009                            &mut update_inner,
1010                            &mut tangent_inner,
1011                            &mut factorization,
1012                        )
1013                        .map_err(|error| format!("{error}"))?
1014                    } else {
1015                        trial_local.decrement_from_chained(
1016                            &mut trial_multipliers_local,
1017                            &(&decrement_inner * step),
1018                        )
1019                    }
1020                    Ok(function(&trial_global, &trial_local)?
1021                        + penalty
1022                            * (violation(
1023                                &constraint_matrix_global,
1024                                &constraint_rhs_global,
1025                                &trial_global,
1026                            ) + violation(
1027                                &constraint_matrix_local,
1028                                &constraint_rhs_local,
1029                                &trial_local,
1030                            )))
1031                },
1032                value,
1033                slope,
1034            )?
1035        };
1036        if step_size == 1.0 {
1037            global.decrement_from_chained(&mut multipliers_global, &decrement_outer);
1038            local.decrement_from_chained(&mut multipliers_local, &decrement_inner)
1039        } else {
1040            global.decrement_from_chained(&mut multipliers_global, &(&decrement_outer * step_size));
1041            local.decrement_from_chained(&mut multipliers_local, &(&decrement_inner * step_size))
1042        }
1043    }
1044}
1045
1046fn unconstrained<J, H, X, E>(
1047    newton_raphson: &NewtonRaphson,
1048    mut function: impl FnMut(&X) -> Result<Scalar, String>,
1049    mut jacobian: impl FnMut(&X) -> Result<J, String>,
1050    mut hessian: impl FnMut(&X) -> Result<H, String>,
1051    initial_guess: X,
1052    sparse: Option<SparseSolver>,
1053) -> Result<X, OptimizationError>
1054where
1055    H: Hessian,
1056    J: Jacobian,
1057    for<'a> &'a J: Div<H, Output = X>,
1058    J: Erase<Erased = E>,
1059    X: Erase<Erased = E> + Solution,
1060    E: Tensor,
1061    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
1062    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
1063{
1064    let mut decrement;
1065    let mut flattened = Vector::zero(if sparse.is_none() {
1066        0
1067    } else {
1068        initial_guess.size()
1069    });
1070    let mut residual;
1071    let mut solution = initial_guess;
1072    let mut step_size;
1073    let mut steps = 0;
1074    loop {
1075        residual = jacobian(&solution)?;
1076        if newton_raphson.error_norm.apply(&residual) < newton_raphson.abs_tol.residual() {
1077            return Ok(solution);
1078        } else if steps == newton_raphson.max_steps {
1079            return Err(OptimizationError::MaximumStepsReached(
1080                newton_raphson.max_steps,
1081                format!("{newton_raphson:?}"),
1082            ));
1083        } else {
1084            steps += 1;
1085            decrement = if let Some(ref solver) = sparse {
1086                let hess = hessian(&solution)?;
1087                residual.fill_into(&mut flattened);
1088                X::from(solver.solve(|i, j| hess.entry(i, j), &flattened)?)
1089            } else {
1090                &residual / hessian(&solution)?
1091            };
1092            if let TrustRegion::Fixed { radius, norm } = newton_raphson.trust_region {
1093                let size = norm.measure(&decrement);
1094                if size > radius {
1095                    decrement *= radius / size
1096                }
1097            }
1098            step_size = newton_raphson.backtracking_line_search::<X, E>(
1099                |trial: &X, _: Scalar| function(trial),
1100                &mut jacobian,
1101                &solution,
1102                &residual,
1103                &decrement,
1104                1.0,
1105            )?;
1106            if step_size != 1.0 {
1107                decrement *= step_size
1108            }
1109            solution -= decrement
1110        }
1111    }
1112}
1113
1114#[expect(clippy::too_many_arguments)]
1115fn constrained_fixed<J, H, X, E>(
1116    newton_raphson: &NewtonRaphson,
1117    mut function: impl FnMut(&X) -> Result<Scalar, String>,
1118    mut jacobian: impl FnMut(&X) -> Result<J, String>,
1119    mut hessian: impl FnMut(&X) -> Result<H, String>,
1120    mut update: impl FnMut(&X, &Vector, Scalar, bool) -> Result<(), String>,
1121    initial_guess: X,
1122    sparse: Option<SparseSolver>,
1123    indices: Vec<usize>,
1124) -> Result<X, OptimizationError>
1125where
1126    H: Hessian,
1127    J: Jacobian,
1128    J: Erase<Erased = E>,
1129    X: Erase<Erased = E> + Solution,
1130    E: Tensor,
1131    <X as Tensor>::Unit: UnitDiv<<X as Tensor>::Unit, Output = Dimensionless>,
1132    for<'a> &'a X: Mul<Quantity<Dimensionless>, Output = X> + Mul<Scalar, Output = X>,
1133{
1134    let mut applied = Vector::zero(initial_guess.size());
1135    let mut retained = vec![true; initial_guess.size()];
1136    indices.iter().for_each(|&index| retained[index] = false);
1137    let unmap: Vec<usize> = retained
1138        .iter()
1139        .enumerate()
1140        .filter_map(|(index, &keep)| keep.then_some(index))
1141        .collect();
1142    let mut decrement = Vector::zero(unmap.len());
1143    let mut factorization = LuDecomposition::zero(if sparse.is_none() { unmap.len() } else { 0 });
1144    let mut residual;
1145    let mut solution = initial_guess;
1146    let mut step_size;
1147    let mut steps = 0;
1148    loop {
1149        residual = jacobian(&solution)?.retain_from(&retained);
1150        if newton_raphson.error_norm.apply(&residual) < newton_raphson.abs_tol.residual() {
1151            return Ok(solution);
1152        } else if steps == newton_raphson.max_steps {
1153            return Err(OptimizationError::MaximumStepsReached(
1154                newton_raphson.max_steps,
1155                format!("{newton_raphson:?}"),
1156            ));
1157        } else if let Some(ref solver) = sparse {
1158            let hess = hessian(&solution)?;
1159            decrement = solver.solve(|i, j| hess.entry(unmap[i], unmap[j]), &residual)?
1160        } else {
1161            hessian(&solution)?
1162                .retain_from(&retained)
1163                .factorize_lu_into(&mut factorization)?;
1164            factorization.solve_into(&residual, &mut decrement)
1165        }
1166        steps += 1;
1167        limit_decrement(newton_raphson, &mut [(&mut decrement, unmap.len())]);
1168        //
1169        // Spread over the variables it belongs to before anything shortens it,
1170        // so that whatever was eliminated is offered the whole direction and
1171        // the fraction of it being taken, rather than a direction of its own.
1172        //
1173        applied.iter_mut().for_each(|entry| *entry = 0.0);
1174        unmap
1175            .iter()
1176            .zip(decrement.iter())
1177            .for_each(|(&index, decrement_a)| applied[index] = *decrement_a);
1178        step_size = if matches!(newton_raphson.line_search, LineSearch::None) {
1179            1.0
1180        } else if let LineSearch::Error {
1181            cut_back,
1182            max_steps,
1183        } = &newton_raphson.line_search
1184        {
1185            backtrack_errors(
1186                newton_raphson,
1187                |trial_size| {
1188                    let mut trial = solution.clone();
1189                    trial.decrement_from_retained(&retained, &(&decrement * trial_size));
1190                    update(&solution, &applied, trial_size, false).is_ok()
1191                        && jacobian(&trial).is_ok()
1192                },
1193                *cut_back,
1194                *max_steps,
1195            )?
1196        } else {
1197            let jac = jacobian(&solution)?;
1198            let mut decrement_full = &solution * 0.0;
1199            decrement_full.decrement_from_retained(&retained, &decrement);
1200            decrement_full *= -1.0;
1201            newton_raphson.backtracking_line_search::<X, E>(
1202                |trial: &X, step: Scalar| {
1203                    update(&solution, &applied, step, false)?;
1204                    function(trial)
1205                },
1206                &mut jacobian,
1207                &solution,
1208                &jac,
1209                &decrement_full,
1210                1.0,
1211            )?
1212        };
1213        update(&solution, &applied, step_size, true)?;
1214        if step_size != 1.0 {
1215            decrement *= step_size
1216        }
1217        solution.decrement_from_retained(&retained, &decrement)
1218    }
1219}
1220
1221#[expect(clippy::too_many_arguments)]
1222fn constrained<J, H, X>(
1223    newton_raphson: &NewtonRaphson,
1224    mut function: impl FnMut(&X) -> Result<Scalar, String>,
1225    mut jacobian: impl FnMut(&X) -> Result<J, String>,
1226    mut hessian: impl FnMut(&X) -> Result<H, String>,
1227    mut update: impl FnMut(&X, &Vector, Scalar, bool) -> Result<(), String>,
1228    initial_guess: X,
1229    sparse: Option<SparseSolver>,
1230    constraint_matrix: Matrix,
1231    constraint_rhs: Vector,
1232) -> Result<X, OptimizationError>
1233where
1234    H: Hessian,
1235    J: Jacobian,
1236    X: Solution,
1237    for<'a> &'a Matrix: Mul<&'a X, Output = Vector>,
1238{
1239    let mut penalty = 0.0 as Scalar;
1240    let num_variables = initial_guess.size();
1241    let mut applied = Vector::zero(num_variables);
1242    let num_constraints = constraint_rhs.len();
1243    let num_total = num_variables + num_constraints;
1244    let mut decrement = Vector::zero(num_total);
1245    let mut factorization = LuDecomposition::zero(if sparse.is_none() { num_total } else { 0 });
1246    let mut multipliers = Vector::zero(num_constraints);
1247    let mut residual = Vector::zero(num_total);
1248    let mut scales = None;
1249    let mut solution = initial_guess;
1250    let mut tangent = SquareMatrix::zero(if sparse.is_none() { num_total } else { 0 });
1251    if sparse.is_none() {
1252        constraint_matrix
1253            .iter()
1254            .enumerate()
1255            .for_each(|(i, constraint_matrix_i)| {
1256                constraint_matrix_i
1257                    .iter()
1258                    .enumerate()
1259                    .for_each(|(j, constraint_matrix_ij)| {
1260                        tangent[i + num_variables][j] = -constraint_matrix_ij;
1261                        tangent[j][i + num_variables] = -constraint_matrix_ij;
1262                    })
1263            });
1264    }
1265    let mut steps = 0;
1266    loop {
1267        (jacobian(&solution)? - &multipliers * &constraint_matrix).fill_into_chained(
1268            &constraint_rhs - &constraint_matrix * &solution,
1269            &mut residual,
1270        );
1271        if converged(newton_raphson, &residual, num_variables, &mut scales) {
1272            return Ok(solution);
1273        } else if steps == newton_raphson.max_steps {
1274            return Err(OptimizationError::MaximumStepsReached(
1275                newton_raphson.max_steps,
1276                format!("{newton_raphson:?}"),
1277            ));
1278        } else if let Some(ref solver) = sparse {
1279            let hess = hessian(&solution)?;
1280            decrement = solver.solve(
1281                |i, j| {
1282                    if i >= num_variables {
1283                        -constraint_matrix[i - num_variables][j]
1284                    } else if j >= num_variables {
1285                        -constraint_matrix[j - num_variables][i]
1286                    } else {
1287                        hess.entry(i, j)
1288                    }
1289                },
1290                &residual,
1291            )?;
1292        } else {
1293            hessian(&solution)?.fill_into(&mut tangent);
1294            tangent.factorize_lu_into(&mut factorization)?;
1295            factorization.solve_into(&residual, &mut decrement)
1296        }
1297        steps += 1;
1298        limit_decrement(newton_raphson, &mut [(&mut decrement, num_variables)]);
1299        //
1300        // Only the variables are lent out, the multipliers chained onto the end
1301        // of the decrement being of another kind entirely.
1302        //
1303        applied
1304            .iter_mut()
1305            .zip(decrement.iter())
1306            .for_each(|(applied_i, decrement_i)| *applied_i = *decrement_i);
1307        let step_size = if matches!(newton_raphson.line_search, LineSearch::None) {
1308            1.0
1309        } else if let LineSearch::Error {
1310            cut_back,
1311            max_steps,
1312        } = &newton_raphson.line_search
1313        {
1314            backtrack_errors(
1315                newton_raphson,
1316                |trial_size| {
1317                    let mut trial = solution.clone();
1318                    let mut trial_multipliers = multipliers.clone();
1319                    trial
1320                        .decrement_from_chained(&mut trial_multipliers, &(&decrement * trial_size));
1321                    update(&solution, &applied, trial_size, false).is_ok()
1322                        && jacobian(&trial).is_ok()
1323                },
1324                *cut_back,
1325                *max_steps,
1326            )?
1327        } else {
1328            penalty = raise_penalty(
1329                penalty,
1330                multipliers.iter().zip(decrement.iter().skip(num_variables)),
1331            );
1332            let violated = penalty * violation(&constraint_matrix, &constraint_rhs, &solution);
1333            let mut gradient = Vector::zero(num_variables);
1334            jacobian(&solution)?.fill_into(&mut gradient);
1335            let slope = merit_slope(gradient.iter().zip(decrement.iter()), violated);
1336            update(&solution, &applied, 0.0, false)?;
1337            let value = function(&solution)? + violated;
1338            backtrack_penalty(
1339                newton_raphson,
1340                |step| {
1341                    let mut trial = solution.clone();
1342                    let mut trial_multipliers = multipliers.clone();
1343                    trial.decrement_from_chained(&mut trial_multipliers, &(&decrement * step));
1344                    update(&solution, &applied, step, false)?;
1345                    Ok(function(&trial)?
1346                        + penalty * violation(&constraint_matrix, &constraint_rhs, &trial))
1347                },
1348                value,
1349                slope,
1350            )?
1351        };
1352        //
1353        // The increment is lent out whole, before it is applied and before it
1354        // is shortened, so that the eliminated variables take the same fraction
1355        // of their own direction as the retained ones take of theirs.
1356        //
1357        update(&solution, &applied, step_size, true)?;
1358        if step_size != 1.0 {
1359            decrement *= step_size
1360        }
1361        solution.decrement_from_chained(&mut multipliers, &decrement)
1362    }
1363}
conspire/math/optimize/newton_raphson/mod.rs

conspire/math/optimize/newton_raphson/
mod.rs
