QPDualActiveSolver.java
/*
* Licensed to the Hipparchus project under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* The Hipparchus project licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.hipparchus.optim.nonlinear.vector.constrained;
import java.util.ArrayList;
import java.util.List;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.DecompositionSolver;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.optim.OptimizationData;
import org.hipparchus.optim.nonlinear.scalar.ObjectiveFunction;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.Pair;
import org.hipparchus.util.Precision;
/**
* Implements the dual active-set method by Goldfarb and Idnani (1983) for
* solving strictly convex quadratic programs of the form:
*
* <pre>
* minimize (1/2) x^T G x + g0^T x
* subject to CE^T x = ce0 (equality constraints)
* CI^T x >= ci0 (inequality constraints)
* lb <= Ax <= ub (bounded constraints)
* </pre>
*
* <p>
* Uses an incremental QR factorization updater (QRUpdater) for managing active
* constraints and maintains multipliers for dual and primal steps.
* </p>
*
* @see <a href="https://doi.org/10.1137/0603006">Goldfarb and Idnani (1983)</a>
* @since 1.11
*/
public class QPDualActiveSolver extends QPOptimizer {
/** No-error code. */
public static final double ERROR_NONE = 0.0;
/** Error code reported in lambda[0] when the Hessian decomposition fails. */
public static final double ERROR_CHOLESKY_DECOMPOSITION = -1.0;
/** Error code reported in lambda[0] when equality constraints are dependent. */
public static final double ERROR_DEPENDENT_EQUALITIES = -2.0;
/** Error code reported in lambda[0] when max iterations are reached. */
public static final double ERROR_MAX_ITERATIONS = -3.0;
/** Error code reported when numerical issue occurs. */
public static final double ERROR_NUMERICAL = -4.0;
/** Error code reported when no solution is found (infeasible). */
public static final double ERROR_INFEASIBLE = -5.0;
/** Machine epsilon for tolerance checks. */
private static final double EPS = FastMath.ulp(1.0);
/**
* Maximum number of iterations allowed. Will be adjusted based on the problem
* dimension.
*/
private int maxIter;
/** Quadratic function representing 1/2 x^T G x + g0^T x. */
private QuadraticFunction function;
/** Equality constraint data (CE^T x + ce0 = 0). */
private LinearEqualityConstraint eqConstraints;
/** Inequality constraint data (CI^T x + ci0 >= 0). */
private LinearInequalityConstraint iqConstraints;
/** Bounded constraint data (lower <= Ax <= upper). */
private LinearBoundedConstraint bConstraints;
/** QP Settings */
private QPDualActiveSolverOption settings;
/**
* Tolerance for symmetric matrix decomposition.
*
* @since 4.1
*/
private MatrixDecompositionTolerance matrixDecompositionTolerance;
/** Simple constructor.
*/
public QPDualActiveSolver() {
// nothing to do
}
/**
* Parses optimization data to extract the objective function and various
* constraint sets.
*
* @param optData optimization data
*/
@Override
protected void parseOptimizationData(final OptimizationData... optData) {
super.parseOptimizationData(optData);
// Reset QP problem to reuse the same instance of the QP solver
this.maxIter = 1000;
this.function = null;
this.eqConstraints = null;
this.iqConstraints = null;
this.bConstraints = null;
this.settings=new QPDualActiveSolverOption();
this.matrixDecompositionTolerance = new MatrixDecompositionTolerance(EPS);
for (OptimizationData data : optData) {
if (data instanceof ObjectiveFunction) {
function = (QuadraticFunction) ((ObjectiveFunction) data).getObjectiveFunction();
} else if (data instanceof LinearEqualityConstraint) {
eqConstraints = (LinearEqualityConstraint) data;
} else if (data instanceof LinearInequalityConstraint) {
iqConstraints = (LinearInequalityConstraint) data;
} else if (data instanceof LinearBoundedConstraint) {
bConstraints = (LinearBoundedConstraint) data;
} else if (data instanceof MatrixDecompositionTolerance) {
matrixDecompositionTolerance = (MatrixDecompositionTolerance) data;
}
else if(data instanceof QPDualActiveSolverOption)
{
settings = (QPDualActiveSolverOption) data;
}
}
}
/**
* Finds the step length for a primal move toward a violated constraint.
* Modern optimized and numerically stable version.
*
* @param z search direction vector
* @param ai constraint row vector
* @param sv current violation offset (slack/surplus value)
* @param equality true if the constraint is an equality constraint
* @return the primal step size, or Double.POSITIVE_INFINITY if the constraint does not block
*/
private double findPrimalStep(final RealVector z,
final RealVector ai,
final double sv,
final boolean equality) {
double sumA = 0.0;
double sumB = 0.0;
double aiNormSq = 0.0;
double zNormSq = 0.0;
final int dimension = ai.getDimension();
for (int i = 0; i < dimension; i++) {
final double aiValue = ai.getEntry(i);
final double zValue = z.getEntry(i);
final double product = aiValue * zValue;
sumA += product;
sumB += FastMath.abs(product);
aiNormSq += aiValue * aiValue;
zNormSq += zValue * zValue;
}
if (sumB <= Precision.EPSILON) {
return Double.POSITIVE_INFINITY;
}
final double absSumA = FastMath.abs(sumA);
double temp = sumB + 0.1 * absSumA;
double tempA = sumB + 0.2 * absSumA;
if (temp <= sumB || tempA <= temp) {
return Double.POSITIVE_INFINITY;
}
final double sumC =
FastMath.sqrt(aiNormSq) * FastMath.sqrt(zNormSq);
temp = sumC + 0.1 * absSumA;
tempA = sumC + 0.2 * absSumA;
if (temp <= sumC || tempA <= temp) {
return Double.POSITIVE_INFINITY;
}
final double alpha = -sv / sumA;
if (!equality && alpha < 0.0) {
return Double.POSITIVE_INFINITY;
}
return alpha;
}
/**
* Finds the blocking active inequality for the dual move.
*
* If reverseStep is false, the algorithm moves in the usual positive
* direction and an active inequality can block only if r_i > 0.
*
* If reverseStep is true, the algorithm moves in the opposite direction
* and an active inequality can block only if r_i < 0.
*
* The returned step is always a positive length:
*
* alpha = u_i / |r_i|
*
* Active equalities are never removed.
*
* @param u current active multipliers
* @param r dual direction
* @param activeSet active-set global constraint indices
* @param me number of equality constraints
* @param reverseStep true if the primal/dual step is taken in the negative direction
* @return pair containing active-set position to drop and positive blocking length;
* if no blocker exists, returns (-1, +Infinity)
*/
private Pair<Integer, Double> findDualBlockingConstraint(final RealVector u,
final RealVector r,
final List<Integer> activeSet,
final int me,
final boolean reverseStep) {
if (activeSet.isEmpty()) {
return new Pair<>(-1, Double.POSITIVE_INFINITY);
}
double alpha = Double.POSITIVE_INFINITY;
int block = -1;
for (int i = 0; i < activeSet.size(); i++) {
/*
* Only active inequalities can leave.
* Active equalities must never be dropped.
*/
if (activeSet.get(i) < me) {
continue;
}
final double ri = r.getEntry(i);
final double absRi=FastMath.abs(ri);
if (!reverseStep) {
/*
* Forward direction:
*
* u_i(alpha) = u_i - alpha * r_i
*
* Only r_i > 0 can drive u_i to zero.
*/
if (ri <= 0.0) {
continue;
}
} else {
/*
* Reverse direction:
*
* signedStep = -alpha
* u_i(new) = u_i - signedStep * r_i
* = u_i + alpha * r_i
*
* Only r_i < 0 can drive u_i to zero.
*/
if (ri >= 0.0) {
continue;
}
}
final double cand = u.getEntry(i) / FastMath.abs(ri);
if (cand < alpha) {
alpha = cand;
block = i;
}
}
return new Pair<>(block, alpha);
}
/**
* Updates multipliers when adding a constraint.
*
* @param u current multipliers or null
* @param r dual direction
* @param alpha step length
* @param partial new partial multiplier
* @return updated multipliers
*/
private RealVector updateMultipliersOnAddition(final RealVector u,
final RealVector r,
final double alpha,
final double partial) {
if (u.getDimension() == 0) {
RealVector v = new ArrayRealVector(1,partial);
return v;
}
// Original: return u.add(r.mapMultiply(-alpha)).append(partial);
// Optimized: copy once, combine in-place, then append
u.combineToSelf(1.0, -alpha, r);
return u.append(partial);
}
/**
* Updates multipliers when removing a constraint.
*
* @param u current multipliers
* @param r dual direction
* @param alpha step length
* @param dropIndex index to remove
* @return updated multipliers
*/
private RealVector updateMultipliersOnRemoval(final RealVector u,
final RealVector r,
final double alpha,
final int dropIndex) {
if (u.getDimension() == 1) {
return new ArrayRealVector(0, 0);
}
// Original: RealVector tmp = u.add(r.mapMultiply(-alpha));
// Optimized: copy once, combine in-place
u.combineToSelf(1.0, -alpha, r);
int size = u.getDimension();
if (dropIndex == 0) {
return u.getSubVector(1, size - 1);
} else if (dropIndex == size - 1) {
return u.getSubVector(0, size - 1);
}
RealVector head = u.getSubVector(0, dropIndex);
RealVector tail = u.getSubVector(dropIndex + 1, size - dropIndex - 1);
RealVector result= head.append(tail);
return result;
}
/**
* Removes a multiplier at a specific index.
*
* @param u the multiplier vector
* @param dropIndex the index to drop
* @return the vector without the dropped multiplier
*/
private RealVector removeMultiplierAt(final RealVector u, final int dropIndex) {
if (u.getDimension() == 1) {
return new ArrayRealVector(0, 0);
}
if (dropIndex == 0) {
return u.getSubVector(1, u.getDimension() - 1);
}
if (dropIndex == u.getDimension() - 1) {
return u.getSubVector(0, u.getDimension() - 1);
}
final RealVector head = u.getSubVector(0, dropIndex);
final RealVector tail = u.getSubVector(dropIndex + 1, u.getDimension() - dropIndex - 1);
return head.append(tail);
}
/**
// * Finds the most violated constraint using normalized violations and an embedded numerical noise filter.
// * <p>
// * Equality constraints use absolute normalized violation.
// * Inequality/bound constraints use one-sided normalized violation.
// * The noise filter evaluates the exact numerical floating-point stability of the constraint residual
// * relative to the magnitude of the terms involved in its calculation.
// * </p>
// *
// * @param sv current constraint evaluation (residuals)
// * @param weights constraint weights
// * @param blackList dependent constraints to ignore
// * @param activeSet constraints currently in the active set
// * @param me number of equality constraints
// * @param C the constraint matrix
// * @param c0 the constraint constant vector
// * @param x the current primal point
// * @return a Pair containing the index of the most violated constraint and the maximum violation value
// */
private Pair<Integer, Double> mostViolatedConstraintNormalized(final RealVector sv,
final RealVector weights,
final boolean[] active ,
final int me,
final RealMatrix C,
final RealVector c0,
final RealVector x) {
double cvMax = 0.0;
int kViolated = -1;
for (int k = 0; k < sv.getDimension(); k++) {
if (active[k] ) {
continue;
}
final double residual = sv.getEntry(k);
final double absResidual = FastMath.abs(residual);
// 1. Scaled Violation
final double scaledViolation;
if (k < me) {
scaledViolation = absResidual * weights.getEntry(k);
} else {
scaledViolation = -residual * weights.getEntry(k);
}
// 2. Convergence Test: skip if not greater than current maximum
if (scaledViolation <= cvMax) {
continue;
}
final RealVector rowC=C.getRowVector(k);
// 3. --- NOISE FILTER ---
double absSum = FastMath.abs(c0.getEntry(k));
for (int i = 0; i < x.getDimension(); i++) {
absSum += FastMath.abs(rowC.getEntry(i) * x.getEntry(i));
}
double tempA = absSum + absResidual;
if (tempA <= absSum) {
continue; // Noise detected
}
double tempB = absSum + 1.5 * absResidual;
if (tempB <= tempA) {
continue; // Noise detected
}
kViolated = k;
cvMax = scaledViolation;
}
return new Pair<>(kViolated, cvMax);
}
/**
* Main optimization routine.
*
* @return the optimal solution or null if infeasible
*/
@Override
public LagrangeSolution doOptimize() {
RealVector g0 = function.getQ();
double g = function.getD();
int n = function.getP().getColumnDimension();
int p = (eqConstraints != null) ? eqConstraints.dimY():0;
int m1 = (iqConstraints != null) ? iqConstraints.dimY() : 0;
int b1 = (bConstraints != null) ? bConstraints.dimY() : 0;
int m = m1 + 2 * b1;
final int mc = p + m;
RealMatrix C = null;
RealVector c0 = null;
RealVector weights = null;
if (mc > 0) {
QPConstraintSystem record = buildGlobalConstraints(n);
C=record.C;
c0=record.c0;
weights = record.weights;
}
RealMatrix G=null;
RealVector x=null;
RealMatrix L=null;
RealMatrix L1=null;
QRUpdater qrUpdater=null;
if (settings.getMatrixMode() == QPMatrixMode.FULL) {
G = function.getP();
try {
final double eps = matrixDecompositionTolerance.getEpsMatrixDecomposition();
final RegularizedCholeskyDecomposition cholesky =
new RegularizedCholeskyDecomposition(G, settings.getEps(), settings.getEps());
DecompositionSolver solver = cholesky.getSolver();
x = solver.solve(g0).mapMultiply(-1.0);
L = cholesky.getL();
L1 = inverseLowerTriangular(L);
qrUpdater = new QRUpdater(L1);
} catch (MathIllegalArgumentException ex) {
// Matrix is not positive definite, return failure solution
return buildFailureSolution(ERROR_CHOLESKY_DECOMPOSITION);
}
} else if (settings.getMatrixMode() == QPMatrixMode.CHOLESKY) {
L = function.getP();
G = L.multiplyTransposed(L);
L1 = inverseLowerTriangular(L);
x = L1.preMultiply(L1.operate(g0)).mapMultiply(-1.0);
qrUpdater = new QRUpdater(L1);
} else {
L1 = function.getP();
L = inverseLowerTriangular(L1);
G = L.multiplyTransposed(L);
x = L1.preMultiply(L1.operate(g0)).mapMultiply(-1.0);
qrUpdater = new QRUpdater(L1);
}
if (mc == 0) {
double obj = 0.5 * x.dotProduct(G.operate(x)) + g0.dotProduct(x) + g;
return new LagrangeSolution(x, new ArrayRealVector(0, 0), obj);
}
// Maximum iterations adjusted based on problem dimension
this.maxIter = 40 * (n + mc);
List<Integer> active = new ArrayList<>();
final boolean[] activeMask = new boolean[mc];
RealVector u = new ArrayRealVector(0, 0);
RealVector r = new ArrayRealVector(0, 0);
RealVector d = null;
RealVector z = null;
int iteration = 0;
boolean refinementDone = false;
// Active-set loop for all constraints
while (mc != 0 && iteration++ < maxIter) {
// Check Dual Feasibility delete constraint with negative multiplier and recalculate solution
boolean dualFeasible = false;
// Continua finché non è dual-fattibile o non svuotiamo il set
while (!dualFeasible && !active.isEmpty()) {
dualFeasible = true;
int dropIndex = -1;
// // 1. Identifica il primo moltiplicatore negativo puro
for (int j = 0; j < active.size(); j++) {
if (active.get(j) >= p) { // Se è una disuguaglianza
if (u.getEntry(j) < 0.0) {
dropIndex = j;
dualFeasible = false;
break; // Esce al primo negativo incontrato
}
}
}
// 2. Se trovato, effettua il drop e riesegui il KKT da capo
if (dropIndex >= 0) {
qrUpdater.deleteConstraint(dropIndex);
final int removedConstraint = active.remove(dropIndex);
activeMask[removedConstraint] = false;
u=removeMultiplierAt(u, dropIndex);
Pair<RealVector, RealVector> xy = recomputeXY(x, G, g0, C, c0, qrUpdater, active);
x = xy.getFirst();
u = xy.getSecond();
}
}
RealVector sv;
// Evaluate constraints sv = C.preMultiply(x).add(c0);
sv = C.operate(x);
sv.combineToSelf(1.0, 1.0, c0);
// Convergence test: find the most violated NON-active constraint
Pair<Integer, Double> violationInfo = mostViolatedConstraintNormalized(sv, weights, activeMask, p, C, c0, x);
int kViolated = violationInfo.getKey();
double cvMax = violationInfo.getValue();
if (cvMax <= settings.getEps() && refinementDone) {
break; // Solution found
}
// Evaluate convergence: no significantly violated NON-active constraint
if (cvMax <= settings.getEps() && !refinementDone) {
kViolated = -1;
refinementDone = true;
Pair<RealVector, RealVector> xy = recomputeXY(x, G, g0, C, c0, qrUpdater, active);
x = xy.getFirst();
u = xy.getSecond();
}
// Active loop evaluation
while (iteration++ < maxIter) {
if (kViolated < 0) {
break;
}
double t1;
double t2;
double t = 0;
double signedStep=0;
double uPartial = 0;
int dropIndex;
RealVector np;
final int constraintIndex = kViolated;
final boolean equality = constraintIndex < p;
// Dual step loop update multiplier and x (if step is also in primal)
// until primal step is not done
while (iteration++ < maxIter) {
np = C.getRowVector(constraintIndex);
sv.setEntry(constraintIndex, np.dotProduct(x) + c0.getEntry(constraintIndex));
d = qrUpdater.computeD(np);
z = qrUpdater.computeZ(d);
r = qrUpdater.solveR(d);
t1 = findPrimalStep(z, np, sv.getEntry(constraintIndex), equality);
final boolean reverseStep = equality && sv.getEntry(constraintIndex) > 0.0;
t1=FastMath.abs(t1);
Pair<Integer, Double> dualStep = findDualBlockingConstraint(u, r, active, p,reverseStep);
t2 = dualStep.getValue();
dropIndex = dualStep.getKey();
t = FastMath.min(t1, t2);
signedStep = reverseStep ? -t : t;
if (!Double.isFinite(t)) {
return cvMax < this.settings.getEpsRelaxation() ?
buildSolution(x, u, active, G, g0, g, p, m) :
buildFailureSolution(ERROR_INFEASIBLE);
}
if (t1 <= t2) {
break; // Primal full step (exit from dual step loop)
} else {
// Manage dual step
if (t1 < Double.POSITIVE_INFINITY) {
// Step is also in primal x = x.add(z.mapMultiply(t));
x.combineToSelf(1.0, signedStep, z);
}
uPartial += signedStep;
u = updateMultipliersOnRemoval(u, r, signedStep, dropIndex);
qrUpdater.deleteConstraint(dropIndex);
final int removedConstraint = active.remove(dropIndex);
activeMask[removedConstraint] = false;
}
}
// Manage full step
if (active.size() < n && qrUpdater.addConstraint(d)) {
active.add(constraintIndex);
activeMask[constraintIndex] = true;
// Step rimal x = x.add(z.mapMultiply(t));
x.combineToSelf(1.0, signedStep, z);
uPartial += signedStep;
u = updateMultipliersOnAddition(u, r, signedStep, uPartial);
break; // Re-evaluate convergence
} else {
return buildFailureSolution(ERROR_DEPENDENT_EQUALITIES);
}
}
}
if (iteration == maxIter) {
return buildFailureSolution(ERROR_MAX_ITERATIONS);
}
return buildSolution(x, u, active, G, g0, g, p, m);
}
/**
* Builds the final solution object with primal x and multipliers lambda.
*
* @param x solution
* @param u active set multipliers
* @param activeSet active set constraints
* @param G square matrix of weights for quadratic terms
* @param g0 vector of weights for linear terms
* @param g constant term
* @param p number of equalities
* @param m number of inequalities
* @return the optimal solution
*/
private LagrangeSolution buildSolution(final RealVector x,
final RealVector u,
final List<Integer> activeSet,
final RealMatrix G,
final RealVector g0,
final double g,
final int p,
final int m) {
final RealVector lambda = new ArrayRealVector(p + m);
if (!activeSet.isEmpty()) {
for (int i = 0; i < activeSet.size(); i++) {
lambda.setEntry(activeSet.get(i), u.getEntry(i));
}
}
final double value = 0.5 * x.dotProduct(G.operate(x)) + g0.dotProduct(x) + g;
return new LagrangeSolution(x, lambda, value);
}
/**
* Computes the inverse of a lower-triangular matrix via forward
* substitution.
*
* @param L lower triangular matrix
* @return inverse of Lower Triangular Matrix
*/
private RealMatrix inverseLowerTriangular(final RealMatrix L) {
final int n = L.getRowDimension();
final RealMatrix Linv = MatrixUtils.createRealMatrix(n, n);
for (int i = 0; i < n; i++) {
final RealVector e = new ArrayRealVector(n);
e.setEntry(i, 1.0);
MatrixUtils.solveLowerTriangularSystem(L, e);
Linv.setColumnVector(i, e);
}
return Linv;
}
/**
* Helper to build a failure solution structure.
*
* @param errorCode the failure code
* @return a LagrangeSolution representing the failure
*/
private LagrangeSolution buildFailureSolution(final double errorCode) {
return new LagrangeSolution(
new ArrayRealVector(0, 0),
new ArrayRealVector(1, errorCode),
0
);
}
/**
* Incremental recomputation of primal x and active multipliers y after a change
* in the working set (for example after dropping one negative active multiplier).
*
* Unlike the absolute KKT reconstruction, this version starts from the current
* primal point xCurrent and only applies:
*
* 1) an active-space correction to reduce the residuals of the current active set,
* 2) a free-space correction to improve projected stationarity,
* 3) a recomputation of the active multipliers consistent with the corrected x.
*
* Convention:
* - objective: 0.5 * x^T G x + g0^T x, with G = L*L^T
* - constraints: C^T x + c0 >= 0
*
* @param x the current primal point
* @param G the quadratic objective matrix
* @param g0 the linear objective vector
* @param C the constraint matrix
* @param c0 the constraint constants
* @param qrUpdater the factorization updater
* @param active the current active set
* @return a Pair containing the corrected primal x and the active multipliers y
*/
private Pair<RealVector, RealVector> recomputeXY(final RealVector x,
final RealMatrix G,
final RealVector g0,
final RealMatrix C,
final RealVector c0,
final QRUpdater qrUpdater,
final List<Integer> active) {
final int nAct = active.size();
// --------------------------------------------------------------
// 1) Active-space correction: reduce active residuals starting from xCurrent
// rp_i = -(a_i^T x + c0_i)
// --------------------------------------------------------------
if (nAct > 0) {
final double[] rpData = new double[nAct];
for (int i = 0; i < nAct; i++) {
final int idx = active.get(i);
final RealVector ai = C.getRowVector(idx);
rpData[i] = -(ai.dotProduct(x) + c0.getEntry(idx));
}
final RealVector rp = new ArrayRealVector(rpData, false);
final RealVector tActive = qrUpdater.solveRT(rp);
if (tActive == null) {
return null; // singular / numerically unsafe
}
// Original: xRec = xRec.add(qrUpdater.applyJActive(tActive));
// Optimized: in-place addition to avoid object allocation
x.combineToSelf(1.0, 1.0, qrUpdater.applyJActive(tActive));
}
// --------------------------------------------------------------
// 2) Free-space correction: improve projected stationarity
// grad = G*x + g0
// --------------------------------------------------------------
// Original: final RealVector gMid = G.operate(xRec).add(g0);
// Optimized: allocate once for G.operate(xRec), then combine in-place
final RealVector gMid = G.operate(x);
gMid.combineToSelf(1.0, 1.0, g0);
final RealVector dMid = qrUpdater.computeD(gMid);
// Original: xRec = xRec.subtract(qrUpdater.computeZ(dMid));
// Optimized: in-place subtraction (xRec = 1.0 * xRec + (-1.0) * Z)
x.combineToSelf(1.0, -1.0, qrUpdater.computeZ(dMid));
// --------------------------------------------------------------
// 3) Recompute active multipliers from corrected xRec
// --------------------------------------------------------------
if (nAct == 0) {
return new Pair<>(x, new ArrayRealVector(0));
}
RealVector yActive = qrUpdater.solveR(dMid);
if (yActive == null) {
return null;
}
if (yActive.getDimension() > nAct) {
yActive = yActive.getSubVector(0, nAct);
}
return new Pair<>(x, yActive);
}
/**
* Private container for the integrated QP system.
* Designed to be stack-allocated.
*/
private static final class QPConstraintSystem {
/** Global constraint matrix C (n x mc) where columns are constraint gradients. */
private final RealMatrix C;
/** Right-hand side vector c0 for the QP problem. */
private final RealVector c0;
/** Normalization weights based on the 2-norm of each constraint gradient. */
private final RealVector weights;
private QPConstraintSystem(RealMatrix C, RealVector c0, RealVector weights) {
this.C = C;
this.c0 = c0;
this.weights = weights;
}
}
/**
* Builds the virtual row-oriented QP constraint system.
*
* <p>The global constraint matrix is represented as:</p>
*
* <pre>
* A = [ Ae ]
* [ Ai ]
* [ Ab ]
* [-Ab ]
* </pre>
*
* <p>No global physical matrix and no transpose are created.</p>
*
* @param n number of primal variables
* @return integrated constraint system
*/
private QPConstraintSystem buildGlobalConstraints(final int n) {
final int p = eqConstraints != null ? eqConstraints.dimY() : 0;
final int m1 = iqConstraints != null ? iqConstraints.dimY() : 0;
final int b1 = bConstraints != null ? bConstraints.dimY() : 0;
final int mc = p + m1 + 2 * b1;
/*
* Original row-oriented matrices.
*
* No transpose and no coefficient copy are performed.
*/
final RealMatrix Ae = eqConstraints != null ? eqConstraints.getA() : null;
final RealMatrix Ai = iqConstraints != null ? iqConstraints.jacobian(null) : null;
final RealMatrix Ab = bConstraints != null ? bConstraints.jacobian(null) : null;
/*
* Virtual global matrix:
*
* [ Ae ]
* A = [ Ai ]
* [ Ab ]
* [-Ab ]
*/
final RealMatrix A = new QPConstraintMatrix(n,Ae, Ai, Ab);
final double[] c0Data = new double[mc];
final double[] wData = new double[mc];
final double safeMin = Precision.EPSILON;
/*
* Equality constraints:
*
* Ae_j x - be_j = 0
*/
if (p > 0) {
final RealVector be = eqConstraints.getLowerBound();
for (int j = 0; j < p; j++) {
c0Data[j] = -be.getEntry(j);
double sumSq = 0.0;
for (int i = 0; i < n; i++) {
final double value = Ae.getEntry(j, i);
sumSq += value * value;
}
final double norm = FastMath.sqrt(sumSq);
wData[j] = norm > safeMin ?
1.0 / norm : 0.0;
}
}
/*
* Inequality constraints:
*
* Ai_j x - bi_j >= 0
*/
if (m1 > 0) {
final RealVector bi = iqConstraints.getLowerBound();
for (int j = 0; j < m1; j++) {
final int globalIndex = p + j;
c0Data[globalIndex] = -bi.getEntry(j);
double sumSq = 0.0;
for (int i = 0; i < n; i++) {
final double value = Ai.getEntry(j, i);
sumSq += value * value;
}
final double norm = FastMath.sqrt(sumSq);
wData[globalIndex] = norm > safeMin ?
1.0 / norm : 0.0;
}
}
/*
* Bounded constraints:
*
* Lower:
*
* Ab_j x - lb_j >= 0
*
* Upper:
*
* -Ab_j x + ub_j >= 0
*
* Both rows have the same Euclidean norm.
*/
if (b1 > 0) {
final RealVector lb = bConstraints.getLowerBound();
final RealVector ub = bConstraints.getUpperBound();
final int lowerOffset = p + m1;
final int upperOffset = lowerOffset + b1;
for (int j = 0; j < b1; j++) {
final int lowerIndex = lowerOffset + j;
final int upperIndex = upperOffset + j;
c0Data[lowerIndex] = -lb.getEntry(j);
c0Data[upperIndex] = ub.getEntry(j);
double sumSq = 0.0;
for (int i = 0; i < n; i++) {
final double value = Ab.getEntry(j, i);
sumSq += value * value;
}
final double norm = FastMath.sqrt(sumSq);
final double inverseNorm = norm > safeMin ?
1.0 / norm : 0.0;
wData[lowerIndex] = inverseNorm;
wData[upperIndex] = inverseNorm;
}
}
return new QPConstraintSystem(
A,
new ArrayRealVector(c0Data, false),
new ArrayRealVector(wData, false));
}
}