void addDiagFx(int offset, ADDvector x, SparceMatrix& lhs, Plan& P, bool phase1 = false) {
ADDvector f_x = P.F(x, phase1);
for(int i = 0; i < x.count(); i++) {
lhs.set(offset, offset, -f_x[i]);
offset++;
}
}
void addR_Cent(ADDvector& r_t, Plan& P, ADDvector& x, ADDvector& lambda, bool phase1 = false) {
int t = 1;
ADDvector R_Cent = -diag(lambda)*P.F(x, phase1) - ConstVector(1/t, lambda.count());
for(int i = 0; i < R_Cent.count(); i++) {
r_t[x.count() + i] = R_Cent[i];
}
}
bool checkTeminationConditions(Plan& P, ADDvector& x, ADDvector& lambda, ADDvector& nu, bool phase1) {
double epsilon_feas = .01;
double epsilon = .01;
ADDvector r_t = R_T(P, x, lambda, nu, phase1);
ADD dist_r_pri = CalcDist(r_t, 0, x.count());
ADD dist_r_dual = CalcDist(r_t, x.count() + lambda.count(), r_t.count());
ADD lambdaTest = dotProduct(-P.F(x, phase1), lambda);
return maximum(dist_r_pri) < epsilon_feas &&
maximum(dist_r_dual) < epsilon_feas &&
maximum(lambdaTest) < epsilon;
}