template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::SetStdCostRegul(double aCoeff,double aCste,int aVmin)
{
for (int anX=X0(); anX<X1() ; anX++)
for (int anY=Y0(); anY<Y1() ; anY++)
for (int aZ = ZMin(anX,anY); aZ< ZMax(anX,anY) ; aZ++)
{
cRoyPt aP1 (anX,anY,aZ);
cCRNode & aS1 = NodeOfP(aP1);
int aC1 = aS1.ResidualFlow(mDirZPlus);
for (int anEdg=0; anEdg<NbEdges() ; anEdg++)
{
if (aS1.EdgeIsValide(anEdg) && (!tabCRIsVertical[anEdg]))
{
cRoyPt aP2(aP1,anEdg);
cCRNode & aS2 = NodeOfP(aP2);
int aC2 = aS2.ResidualFlow(mDirZPlus);
double aCost = aCste + aCoeff*(aC1+aC2)/2.0;
if (Cnx8())
aCost *= tabCRIsArcV8[anEdg] ? 0.2928 : 0.4142 ;
int iCost = int(aCost+0.5);
if (iCost<aVmin)
iCost = aVmin;
aS1.SetResidualFlow(anEdg,iCost);
}
}
}
}
int Rsimp(int m, int n, double **A, double *b, double *c,
double *x, int *basis, int *nonbasis,
double **R, double **Q, double *t1, double *t2){
int i,j,k,l,q,qv;
int max_steps=20;
double r,a,at;
void GQR(int,int,double**,double**);
max_steps=4*n;
for(k=0; k<=max_steps;k++){
/*
++ Step 0) load new basis matrix and factor it
*/
for(i=0;i<m;i++)for(j=0;j<m;j++)R0(i,j)=AB0(i,j);
GQR(m,m,Q,R);
/*
++ Step 1) solving system B'*w=c(basis)
++ a) forward solve R'*y=c(basis)
*/
for(i=0;i<m;i++){
Y0(i)=0.0;
for(j=0;j<i;j++)Y0(i)+=R0(j,i)*Y0(j);
if (R0(i,i)!=0.0) Y0(i)=(CB0(i)-Y0(i))/R0(i,i);
else {
printf("Warning Singular Matrix Found\n");
return LP_FAIL;
}
}
/*
++ b) find w=Q*y
++ note: B'*w=(Q*R)'*Q*y= R'*(Q'*Q)*y=R'*y=c(basis)
*/
for(i=0;i<m;i++){
W0(i)=0.0;
for(j=0;j<m;j++)W0(i)+=Q0(i,j)*Y0(j);
}
/*
++ Step 2)find entering variable,
++ (use lexicographically first variable with negative reduced cost)
*/
q=n;
for(i=0;i<n-m;i++){
/* calculate reduced cost */
r=CN0(i);
for(j=0;j<m;j++) r-=W0(j)*AN0(j,i);
if (r<-zero_tol && (q==n || nonbasis0(i)<nonbasis0(q))) q=i;
}
/*
++ if ratios were all nonnegative current solution is optimal
*/
if (q==n){
if (verbose>0) printf("optimal solution found in %d iterations\n",k);
return LP_OPT;
}
/*
++ Step 3)Calculate translation direction for q entering
++ by solving system B*d=-A(:,nonbasis(q));
++ a) let y=-Q'*A(:,nonbasis(q));
*/
for(i=0;i<m;i++){
Y0(i)=0.0;
for(j=0;j<m;j++) Y0(i)-=Q0(j,i)*AN0(j,q);
}
/*
++ b) back solve Rd=y (d=R\y)
++ note B*d= Q*R*d=Q*y=Q*-Q'*A(:nonbasis(q))=-A(:,nonbasis(q))
*/
for(i=m-1;i>=0;i--){
D0(i)=0.0;
for(j=m-1;j>=i+1;j--)D0(i)+=R0(i,j)*D0(j);
if (R0(i,i)!=0.0) D0(i)=(Y0(i)-D0(i))/R0(i,i);
else {
printf("Warning Singular Matrix Found\n");
return LP_FAIL;
}
}
/*
++ Step 4 Choose leaving variable
++ (first variable to become negative, by moving in direction D)
++ (if none become negative, then objective function unbounded)
*/
a=0;
l=-1;
for(i=0;i<m;i++){
if (D0(i)<-zero_tol){
at=-1*XB0(i)/D0(i);
if (l==-1 || at<a){ a=at; l=i;}
}
}
if (l==-1){
if (verbose>0){
printf("Objective function Unbounded (%d iterations)\n",k);
}
return LP_UNBD;
}
/*
++ Step 5) Update solution and basis data
*/
XN0(q)=a;
for(j=0;j<m;j++) XB0(j)+=a*D0(j);
XB0(l)=0.0; /* enforce strict zeroness of nonbasis variables */
qv=nonbasis0(q);
nonbasis0(q)=basis0(l);
basis0(l)=qv;
}
if (verbose>=0){
printf("Simplex Algorithm did not Terminate in %d iterations\n",k);
}
return LP_FAIL;
}
void RKIntegratorInternal::init(){
// Call the base class init
IntegratorInternal::init();
casadi_assert_message(nq_==0, "Quadratures not supported.");
// Number of finite elements
int nk = getOption("number_of_finite_elements");
// Interpolation order
int deg = getOption("interpolation_order");
casadi_assert_message(deg==1, "Not implemented");
// Expand f?
bool expand_f = getOption("expand_f");
// Size of the finite elements
double h = (tf_-t0_)/nk;
// MX version of the same
MX h_mx = h;
// Initial state
MX Y0("Y0",nx_);
// Free parameters
MX P("P",np_);
// Current state
MX Y = Y0;
// Dummy time
MX T = 0;
// Integrate until the end
for(int k=0; k<nk; ++k){
// Call the ode right hand side function
vector<MX> f_in(DAE_NUM_IN);
f_in[DAE_T] = T;
f_in[DAE_X] = Y;
f_in[DAE_P] = P;
vector<MX> f_out = f_.call(f_in);
MX ode_rhs = f_out[DAE_ODE];
// Explicit Euler step
Y += h_mx*ode_rhs;
}
// Create a function which returns the state at the end of the time horizon
vector<MX> yf_in(2);
yf_in[0] = Y0;
yf_in[1] = P;
MXFunction yf_fun(yf_in,Y);
// Should the function be expanded in elementary operations?
if(expand_f){
yf_fun.init();
yf_fun_ = SXFunction(yf_fun);
} else {
yf_fun_ = yf_fun;
}
// Set number of derivative directions
yf_fun_.setOption("number_of_fwd_dir",getOption("number_of_fwd_dir"));
yf_fun_.setOption("number_of_adj_dir",getOption("number_of_adj_dir"));
// Initialize function
yf_fun_.init();
}
