void addR_Dual(ADDvector& r_t, Plan& P, ADDvector& x, ADDvector& lambda, ADDvector& mu, bool phase1 = false) {
ADDvector R_Dual = P.DeltaF_0(x, phase1) + P.DF_x(phase1).Transpose() * lambda + P.A()->Transpose() * mu;
for(int i = 0; i < R_Dual.count(); i++) {
r_t[i] = R_Dual[i];
}
}
void addDF_x(int offset, ADDvector& x, ADDvector& lambda, SparceMatrix& lhs, Plan& P, bool phase1 = false) {
SparceMatrix DF_x = P.DF_x(phase1);
DF_x.Transpose().insertWithOffset(lhs, 0, offset);
(-diag(lambda)*DF_x).insertWithOffset(lhs, offset, 0);
}