void addD2(SparceMatrix& lhs, ADDvector& x, Plan& P, bool phase1 = false) {
if(phase1)
return; //The second dirivative of s is 0. So leave it alone.
int nSteps = P.getNumSteps();
for(int i = 0; i < nSteps; i++) {
ADD tmp = x[i] - x[i+nSteps];
ADD val = mgr.addOne().Divide(tmp*tmp);
lhs.set(i,i,-val);
lhs.set(i,i+nSteps,-val);
lhs.set(i+nSteps,i,val);
lhs.set(i+nSteps,i+nSteps,val);
}
}
double evaluate(Plan& P) {
//Compute Phase One
ADDvector xPhase1(P.getNumStepsWithUnknownDurration() + P.getNumSteps() + P.getNumSlackVars() + 1);
for(int i = 0; i < xPhase1.count(); i++) {
xPhase1[i] = mgr.addZero();
}
xPhase1[xPhase1.count()-1] = mgr.constant(10000); //inital value for s
ADDvector lambda(P.getNumFunctions());
for(int i = 0; i < lambda.count(); i++) {
lambda[i] = mgr.addZero();
}
ADDvector nu(P.getNumFunctions());
for(int i = 0; i < nu.count(); i++) {
nu[i] = mgr.addZero();
}
while(checkTeminationConditions(P, xPhase1, lambda, nu, true)) {
ADDvector DxPhase1(xPhase1.count());
ADDvector Dlambda(lambda.count());
ADDvector Dnu(nu.count());
calcStepDirection( xPhase1, lambda, nu,
DxPhase1, Dlambda, Dnu,
P, true);
takeStep( xPhase1, lambda, nu,
DxPhase1, Dlambda, Dnu);
}
//Compute Phase two
ADDvector x(xPhase1.count() -1);
for(int i = 0; i < x.count(); i++) {
x[i] = xPhase1[i];
}
while(checkTeminationConditions(P, x, lambda, nu, false)) {
ADDvector Dx(x.count());
ADDvector Dlambda(lambda.count());
ADDvector Dnu(nu.count());
calcStepDirection( x, lambda, nu,
Dx, Dlambda, Dnu,
P, false);
takeStep( x, lambda, nu,
Dx, Dlambda, Dnu);
}
//Really I need to do some max's and sum's here but that can be added later.
//Right now I need to add something to test all of this code.
return 0;
}