returnValue LSQEndTerm::evaluateSensitivitiesGN( BlockMatrix *GNhessian ){
int run2, run3, run4;
const int N = grid.getNumPoints();
const int nh = fcn.getDim();
if( bSeed != 0 ){
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
double *bseed = new double [nh];
double **J = new double*[nh];
for( run2 = 0; run2 < nh; run2++ )
J[run2] = new double[fcn.getNumberOfVariables() +1];
if( bSeed->getNumRows( 0, 0 ) != 1 ) return ACADOWARNING( RET_WRONG_DEFINITION_OF_SEEDS );
Matrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
Matrix Dx ( 1, nx );
Matrix Dxa( 1, na );
Matrix Dp ( 1, np );
Matrix Du ( 1, nu );
Matrix Dw ( 1, nw );
Dx .setZero();
Dxa.setZero();
Dp .setZero();
Du .setZero();
Dw .setZero();
for( run2 = 0; run2 < nh; run2++ ) bseed[run2] = 0;
for( run2 = 0; run2 < nh; run2++ ){
for(run3 = 0; (int) run3 < fcn.getNumberOfVariables() +1; run3++ )
J[run2][run3] = 0.0;
bseed[run2] = 1.0;
fcn.AD_backward( 0, bseed, J[run2] );
bseed[run2] = 0.0;
for( run3 = 0; run3 < nx; run3++ ){
Dx( 0, run3 ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx; run3 < nx+na; run3++ ){
Dxa( 0, run3-nx ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na; run3 < nx+na+np; run3++ ){
Dp( 0, run3-nx-na ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na+np; run3 < nx+na+np+nu; run3++ ){
Du( 0, run3-nx-na-np ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
for( run3 = nx+na+np+nu; run3 < nx+na+np+nu+nw; run3++ ){
Dw( 0, run3-nx-na-np-nu ) += bseed_(0,0)*J[run2][y_index[run3]]*S_h_res[run2];
}
}
if( nx > 0 ) dBackward.setDense( 0, N-1, Dx );
if( na > 0 ) dBackward.setDense( 0, 2*N-1, Dxa );
if( np > 0 ) dBackward.setDense( 0, 3*N-1, Dp );
if( nu > 0 ) dBackward.setDense( 0, 4*N-1, Du );
if( nw > 0 ) dBackward.setDense( 0, 5*N-1, Dw );
// COMPUTE GAUSS-NEWTON HESSIAN APPROXIMATION IF REQUESTED:
// --------------------------------------------------------
if( GNhessian != 0 ){
const int nnn = nx+na+np+nu+nw;
Matrix tmp( nh, nnn );
for( run3 = 0; run3 < nnn; run3++ ){
for( run2 = 0; run2 < nh; run2++ ){
tmp( run2, run3 ) = 0.0;
for( run4 = 0; run4 < nh; run4++ ){
tmp( run2, run3 ) += S.operator()(run2,run4)*J[run4][y_index[run3]];
}
}
}
Matrix tmp2;
int i,j;
int *Sidx = new int[6];
int *Hidx = new int[5];
Sidx[0] = 0;
Sidx[1] = nx;
Sidx[2] = nx+na;
Sidx[3] = nx+na+np;
Sidx[4] = nx+na+np+nu;
Sidx[5] = nx+na+np+nu+nw;
Hidx[0] = N-1;
Hidx[1] = 2*N-1;
Hidx[2] = 3*N-1;
Hidx[3] = 4*N-1;
Hidx[4] = 5*N-1;
for( i = 0; i < 5; i++ ){
for( j = 0; j < 5; j++ ){
tmp2.init(Sidx[i+1]-Sidx[i],Sidx[j+1]-Sidx[j]);
tmp2.setZero();
for( run3 = Sidx[i]; run3 < Sidx[i+1]; run3++ )
for( run4 = Sidx[j]; run4 < Sidx[j+1]; run4++ )
for( run2 = 0; run2 < nh; run2++ )
tmp2(run3-Sidx[i],run4-Sidx[j]) += J[run2][y_index[run3]]*tmp(run2,run4);
if( tmp2.getDim() != 0 ) GNhessian->addDense(Hidx[i],Hidx[j],tmp2);
}
}
delete[] Sidx;
delete[] Hidx;
}
// --------------------------------------------------------
for( run2 = 0; run2 < nh; run2++ )
delete[] J[run2];
delete[] J;
delete[] bseed;
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
returnValue PointConstraint::evaluateSensitivities( const DMatrix &seed, BlockMatrix &hessian ){
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
int run1, run2;
if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
const int nc = fcn[0].getDim();
const int N = grid.getNumPoints();
ASSERT( (int) seed.getNumRows() == nc );
double *bseed1 = new double[nc];
double *bseed2 = new double[nc];
double *R = new double[nc];
double *J = new double[fcn[0].getNumberOfVariables() +1];
double *H = new double[fcn[0].getNumberOfVariables() +1];
double *fseed = new double[fcn[0].getNumberOfVariables() +1];
for( run1 = 0; run1 < nc; run1++ ){
bseed1[run1] = seed(run1,0);
bseed2[run1] = 0.0;
}
for( run1 = 0; run1 < fcn[0].getNumberOfVariables()+1; run1++ )
fseed[run1] = 0.0;
dBackward.init( 1, 5*N );
DMatrix Dx ( nc, nx );
DMatrix Dxa( nc, na );
DMatrix Dp ( nc, np );
DMatrix Du ( nc, nu );
DMatrix Dw ( nc, nw );
DMatrix Hx ( nx, nx );
DMatrix Hxa( nx, na );
DMatrix Hp ( nx, np );
DMatrix Hu ( nx, nu );
DMatrix Hw ( nx, nw );
for( run2 = 0; run2 < nx; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed, R );
for( run1 = 0; run1 < nc; run1++ )
Dx( run1, run2 ) = R[run1];
fseed[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2, run1 ) = -H[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2, run1-nx ) = -H[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2, run1-nx-na ) = -H[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2, run1-nx-na-np ) = -H[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
}
if( nx > 0 ){
dBackward.setDense( 0, point_index, Dx );
if( nx > 0 ) hessian.addDense( point_index, point_index, Hx );
if( na > 0 ) hessian.addDense( point_index, N + point_index, Hxa );
if( np > 0 ) hessian.addDense( point_index, 2*N + point_index, Hp );
if( nu > 0 ) hessian.addDense( point_index, 3*N + point_index, Hu );
if( nw > 0 ) hessian.addDense( point_index, 4*N + point_index, Hw );
}
Hx.init ( na, nx );
Hxa.init( na, na );
Hp.init ( na, np );
Hu.init ( na, nu );
Hw.init ( na, nw );
for( run2 = nx; run2 < nx+na; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed, R );
for( run1 = 0; run1 < nc; run1++ )
Dxa( run1, run2-nx ) = R[run1];
fseed[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx, run1 ) = -H[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx, run1-nx ) = -H[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx, run1-nx-na ) = -H[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx, run1-nx-na-np ) = -H[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
}
if( na > 0 ){
dBackward.setDense( 0, N+point_index, Dxa );
if( nx > 0 ) hessian.addDense( N+point_index, point_index, Hx );
if( na > 0 ) hessian.addDense( N+point_index, N + point_index, Hxa );
if( np > 0 ) hessian.addDense( N+point_index, 2*N + point_index, Hp );
if( nu > 0 ) hessian.addDense( N+point_index, 3*N + point_index, Hu );
if( nw > 0 ) hessian.addDense( N+point_index, 4*N + point_index, Hw );
}
Hx.init ( np, nx );
Hxa.init( np, na );
Hp.init ( np, np );
Hu.init ( np, nu );
Hw.init ( np, nw );
for( run2 = nx+na; run2 < nx+na+np; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed, R );
for( run1 = 0; run1 < nc; run1++ )
Dp( run1, run2-nx-na ) = R[run1];
fseed[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na, run1 ) = -H[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na, run1-nx ) = -H[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na, run1-nx-na ) = -H[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np ) = -H[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
}
if( np > 0 ){
dBackward.setDense( 0, 2*N+point_index, Dp );
if( nx > 0 ) hessian.addDense( 2*N+point_index, point_index, Hx );
if( na > 0 ) hessian.addDense( 2*N+point_index, N + point_index, Hxa );
if( np > 0 ) hessian.addDense( 2*N+point_index, 2*N + point_index, Hp );
if( nu > 0 ) hessian.addDense( 2*N+point_index, 3*N + point_index, Hu );
if( nw > 0 ) hessian.addDense( 2*N+point_index, 4*N + point_index, Hw );
}
Hx.init ( nu, nx );
Hxa.init( nu, na );
Hp.init ( nu, np );
Hu.init ( nu, nu );
Hw.init ( nu, nw );
for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed, R );
for( run1 = 0; run1 < nc; run1++ )
Du( run1, run2-nx-na-np ) = R[run1];
fseed[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np, run1 ) = -H[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np, run1-nx ) = -H[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na ) = -H[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np ) = -H[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
}
if( nu > 0 ){
dBackward.setDense( 0, 3*N+point_index, Du );
if( nx > 0 ) hessian.addDense( 3*N+point_index, point_index, Hx );
if( na > 0 ) hessian.addDense( 3*N+point_index, N + point_index, Hxa );
if( np > 0 ) hessian.addDense( 3*N+point_index, 2*N + point_index, Hp );
if( nu > 0 ) hessian.addDense( 3*N+point_index, 3*N + point_index, Hu );
if( nw > 0 ) hessian.addDense( 3*N+point_index, 4*N + point_index, Hw );
}
Hx.init ( nw, nx );
Hxa.init( nw, na );
Hp.init ( nw, np );
Hu.init ( nw, nu );
Hw.init ( nw, nw );
for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed, R );
for( run1 = 0; run1 < nc; run1++ )
Dw( run1, run2-nx-na-np-nu ) = R[run1];
fseed[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np-nu, run1 ) = -H[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx ) = -H[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na ) = -H[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np ) = -H[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np-nu, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
}
if( nw > 0 ){
dBackward.setDense( 0, 4*N+point_index, Dw );
if( nx > 0 ) hessian.addDense( 4*N+point_index, point_index, Hx );
if( na > 0 ) hessian.addDense( 4*N+point_index, N + point_index, Hxa );
if( np > 0 ) hessian.addDense( 4*N+point_index, 2*N + point_index, Hp );
if( nu > 0 ) hessian.addDense( 4*N+point_index, 3*N + point_index, Hu );
if( nw > 0 ) hessian.addDense( 4*N+point_index, 4*N + point_index, Hw );
}
delete[] bseed1;
delete[] bseed2;
delete[] R ;
delete[] J ;
delete[] H ;
delete[] fseed ;
return SUCCESSFUL_RETURN;
}
returnValue PointConstraint::evaluateSensitivities( ){
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
int run1;
returnValue returnvalue;
if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
const int N = grid.getNumPoints();
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
if( bSeed != 0 ){
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
int nBDirs = bSeed->getNumRows( 0, 0 );
DMatrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
DMatrix Dx ( nBDirs, nx );
DMatrix Dxa( nBDirs, na );
DMatrix Dp ( nBDirs, np );
DMatrix Du ( nBDirs, nu );
DMatrix Dw ( nBDirs, nw );
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[0].AD_backward( bseed_.getRow(run1), JJ[0] ) );
if( nx > 0 ) Dx .setRow( run1, JJ[0].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[0].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[0].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[0].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[0].getW () );
JJ[0].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, point_index , Dx );
if( na > 0 )
dBackward.setDense( 0, N+point_index, Dxa );
if( np > 0 )
dBackward.setDense( 0, 2*N+point_index, Dp );
if( nu > 0 )
dBackward.setDense( 0, 3*N+point_index, Du );
if( nw > 0 )
dBackward.setDense( 0, 4*N+point_index, Dw );
return SUCCESSFUL_RETURN;
}
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ){
if( bSeed != 0 ) return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
if( condType != CT_SPARSE ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
dForward.init( 1, 5*N );
if( xSeed != 0 ){
DMatrix tmp;
xSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( 0, 0, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( xaSeed != 0 ){
DMatrix tmp;
xaSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx, N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( pSeed != 0 ){
DMatrix tmp;
pSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na, 2*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( uSeed != 0 ){
DMatrix tmp;
uSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na+np, 3*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( wSeed != 0 ){
DMatrix tmp;
wSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na+np+nu, 4*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
returnValue BoundaryConstraint::evaluateSensitivities( const DMatrix &seed, BlockMatrix &hessian ){
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
int run1, run2;
const int nc = getNC();
const int N = grid.getNumPoints();
ASSERT( (int) seed.getNumRows() == nc );
double *bseed1 = new double[nc];
double *bseed2 = new double[nc];
double *R = new double[nc];
double *J1 = new double[fcn[0].getNumberOfVariables() +1];
double *H1 = new double[fcn[0].getNumberOfVariables() +1];
double *fseed1 = new double[fcn[0].getNumberOfVariables() +1];
double *J2 = new double[fcn[1].getNumberOfVariables() +1];
double *H2 = new double[fcn[1].getNumberOfVariables() +1];
double *fseed2 = new double[fcn[1].getNumberOfVariables() +1];
for( run1 = 0; run1 < nc; run1++ ){
bseed1[run1] = seed(run1,0);
bseed2[run1] = 0.0;
}
for( run1 = 0; run1 < fcn[0].getNumberOfVariables()+1; run1++ )
fseed1[run1] = 0.0;
for( run1 = 0; run1 < fcn[1].getNumberOfVariables()+1; run1++ )
fseed2[run1] = 0.0;
dBackward.init( 1, 5*N );
DMatrix Dx ( nc, nx );
DMatrix Dxa( nc, na );
DMatrix Dp ( nc, np );
DMatrix Du ( nc, nu );
DMatrix Dw ( nc, nw );
DMatrix Hx ( nx, nx );
DMatrix Hxa( nx, na );
DMatrix Hp ( nx, np );
DMatrix Hu ( nx, nu );
DMatrix Hw ( nx, nw );
for( run2 = 0; run2 < nx; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed1[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed1, R );
for( run1 = 0; run1 < nc; run1++ )
Dx( run1, run2 ) = R[run1];
fseed1[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J1[run1] = 0.0;
H1[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J1, H1 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2, run1 ) = -H1[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2, run1-nx ) = -H1[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2, run1-nx-na ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2, run1-nx-na-np ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H1[y_index[0][run1]];
}
if( nx > 0 ){
dBackward.setDense( 0, 0, Dx );
if( nx > 0 ) hessian.addDense( 0, 0, Hx );
if( na > 0 ) hessian.addDense( 0, N, Hxa );
if( np > 0 ) hessian.addDense( 0, 2*N, Hp );
if( nu > 0 ) hessian.addDense( 0, 3*N, Hu );
if( nw > 0 ) hessian.addDense( 0, 4*N, Hw );
}
Hx.init ( nx, nx );
Hxa.init( nx, na );
Hp.init ( nx, np );
Hu.init ( nx, nu );
Hw.init ( nx, nw );
for( run2 = 0; run2 < nx; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed2[y_index[1][run2]] = 1.0;
fcn[1].AD_forward( 0, fseed2, R );
for( run1 = 0; run1 < nc; run1++ )
Dx( run1, run2 ) = R[run1];
fseed2[y_index[1][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[1].getNumberOfVariables(); run1++ ){
J2[run1] = 0.0;
H2[run1] = 0.0;
}
fcn[1].AD_backward2( 0, bseed1, bseed2, J2, H2 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2, run1 ) = -H2[y_index[1][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2, run1-nx ) = -H2[y_index[1][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2, run1-nx-na ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2, run1-nx-na-np ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H2[y_index[1][run1]];
}
if( nx > 0 ){
dBackward.setDense( 0, N-1, Dx );
if( nx > 0 ) hessian.addDense( N-1, N-1, Hx );
if( na > 0 ) hessian.addDense( N-1, 2*N-1, Hxa );
if( np > 0 ) hessian.addDense( N-1, 3*N-1, Hp );
if( nu > 0 ) hessian.addDense( N-1, 4*N-1, Hu );
if( nw > 0 ) hessian.addDense( N-1, 5*N-1, Hw );
}
Hx.init ( na, nx );
Hxa.init( na, na );
Hp.init ( na, np );
Hu.init ( na, nu );
Hw.init ( na, nw );
for( run2 = nx; run2 < nx+na; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed1[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed1, R );
for( run1 = 0; run1 < nc; run1++ )
Dxa( run1, run2-nx ) = R[run1];
fseed1[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J1[run1] = 0.0;
H1[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J1, H1 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx, run1 ) = -H1[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx, run1-nx ) = -H1[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx, run1-nx-na ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx, run1-nx-na-np ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H1[y_index[0][run1]];
}
if( na > 0 ){
dBackward.setDense( 0, N, Dxa );
if( nx > 0 ) hessian.addDense( N, 0, Hx );
if( na > 0 ) hessian.addDense( N, N, Hxa );
if( np > 0 ) hessian.addDense( N, 2*N, Hp );
if( nu > 0 ) hessian.addDense( N, 3*N, Hu );
if( nw > 0 ) hessian.addDense( N, 4*N, Hw );
}
Hx.init ( na, nx );
Hxa.init( na, na );
Hp.init ( na, np );
Hu.init ( na, nu );
Hw.init ( na, nw );
for( run2 = nx; run2 < nx+na; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed2[y_index[1][run2]] = 1.0;
fcn[1].AD_forward( 0, fseed2, R );
for( run1 = 0; run1 < nc; run1++ )
Dxa( run1, run2-nx ) = R[run1];
fseed2[y_index[1][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[1].getNumberOfVariables(); run1++ ){
J2[run1] = 0.0;
H2[run1] = 0.0;
}
fcn[1].AD_backward2( 0, bseed1, bseed2, J2, H2 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx, run1 ) = -H2[y_index[1][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx, run1-nx ) = -H2[y_index[1][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx, run1-nx-na ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx, run1-nx-na-np ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H2[y_index[1][run1]];
}
if( na > 0 ){
dBackward.setDense( 0, 2*N-1, Dxa );
if( nx > 0 ) hessian.addDense( 2*N-1, N-1, Hx );
if( na > 0 ) hessian.addDense( 2*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian.addDense( 2*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian.addDense( 2*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian.addDense( 2*N-1, 5*N-1, Hw );
}
Hx.init ( np, nx );
Hxa.init( np, na );
Hp.init ( np, np );
Hu.init ( np, nu );
Hw.init ( np, nw );
for( run2 = nx+na; run2 < nx+na+np; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed1[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed1, R );
for( run1 = 0; run1 < nc; run1++ )
Dp( run1, run2-nx-na ) = R[run1];
fseed1[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J1[run1] = 0.0;
H1[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J1, H1 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na, run1 ) = -H1[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na, run1-nx ) = -H1[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na, run1-nx-na ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na, run1-nx-na-np-nu ) = -H1[y_index[0][run1]];
}
if( np > 0 ){
dBackward.setDense( 0, 2*N, Dp );
if( nx > 0 ) hessian.addDense( 2*N, 0, Hx );
if( na > 0 ) hessian.addDense( 2*N, N, Hxa );
if( np > 0 ) hessian.addDense( 2*N, 2*N, Hp );
if( nu > 0 ) hessian.addDense( 2*N, 3*N, Hu );
if( nw > 0 ) hessian.addDense( 2*N, 4*N, Hw );
}
Hx.init ( np, nx );
Hxa.init( np, na );
Hp.init ( np, np );
Hu.init ( np, nu );
Hw.init ( np, nw );
for( run2 = nx+na; run2 < nx+na+np; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed2[y_index[1][run2]] = 1.0;
fcn[1].AD_forward( 0, fseed2, R );
for( run1 = 0; run1 < nc; run1++ )
Dp( run1, run2-nx-na ) = R[run1];
fseed2[y_index[1][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[1].getNumberOfVariables(); run1++ ){
J2[run1] = 0.0;
H2[run1] = 0.0;
}
fcn[1].AD_backward2( 0, bseed1, bseed2, J2, H2 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na, run1 ) = -H2[y_index[1][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na, run1-nx ) = -H2[y_index[1][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na, run1-nx-na ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na, run1-nx-na-np-nu ) = -H2[y_index[1][run1]];
}
if( np > 0 ){
dBackward.setDense( 0, 3*N-1, Dp );
if( nx > 0 ) hessian.addDense( 3*N-1, N-1, Hx );
if( na > 0 ) hessian.addDense( 3*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian.addDense( 3*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian.addDense( 3*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian.addDense( 3*N-1, 5*N-1, Hw );
}
Hx.init ( nu, nx );
Hxa.init( nu, na );
Hp.init ( nu, np );
Hu.init ( nu, nu );
Hw.init ( nu, nw );
for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed1[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed1, R );
for( run1 = 0; run1 < nc; run1++ )
Du( run1, run2-nx-na-np ) = R[run1];
fseed1[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J1[run1] = 0.0;
H1[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J1, H1 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np, run1 ) = -H1[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np, run1-nx ) = -H1[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np, run1-nx-na-np-nu ) = -H1[y_index[0][run1]];
}
if( nu > 0 ){
dBackward.setDense( 0, 3*N, Du );
if( nx > 0 ) hessian.addDense( 3*N, 0, Hx );
if( na > 0 ) hessian.addDense( 3*N, N, Hxa );
if( np > 0 ) hessian.addDense( 3*N, 2*N, Hp );
if( nu > 0 ) hessian.addDense( 3*N, 3*N, Hu );
if( nw > 0 ) hessian.addDense( 3*N, 4*N, Hw );
}
Hx.init ( nu, nx );
Hxa.init( nu, na );
Hp.init ( nu, np );
Hu.init ( nu, nu );
Hw.init ( nu, nw );
for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed2[y_index[1][run2]] = 1.0;
fcn[1].AD_forward( 0, fseed2, R );
for( run1 = 0; run1 < nc; run1++ )
Du( run1, run2-nx-na-np ) = R[run1];
fseed2[y_index[1][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[1].getNumberOfVariables(); run1++ ){
J2[run1] = 0.0;
H2[run1] = 0.0;
}
fcn[1].AD_backward2( 0, bseed1, bseed2, J2, H2 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np, run1 ) = -H2[y_index[1][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np, run1-nx ) = -H2[y_index[1][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np, run1-nx-na-np-nu ) = -H2[y_index[1][run1]];
}
if( nu > 0 ){
dBackward.setDense( 0, 4*N-1, Du );
if( nx > 0 ) hessian.addDense( 4*N-1, N-1, Hx );
if( na > 0 ) hessian.addDense( 4*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian.addDense( 4*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian.addDense( 4*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian.addDense( 4*N-1, 5*N-1, Hw );
}
Hx.init ( nw, nx );
Hxa.init( nw, na );
Hp.init ( nw, np );
Hu.init ( nw, nu );
Hw.init ( nw, nw );
for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed1[y_index[0][run2]] = 1.0;
fcn[0].AD_forward( 0, fseed1, R );
for( run1 = 0; run1 < nc; run1++ )
Dw( run1, run2-nx-na-np-nu ) = R[run1];
fseed1[y_index[0][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
J1[run1] = 0.0;
H1[run1] = 0.0;
}
fcn[0].AD_backward2( 0, bseed1, bseed2, J1, H1 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np-nu, run1 ) = -H1[y_index[0][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx ) = -H1[y_index[0][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np ) = -H1[y_index[0][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np-nu, run1-nx-na-np-nu ) = -H1[y_index[0][run1]];
}
if( nw > 0 ){
dBackward.setDense( 0, 4*N, Dw );
if( nx > 0 ) hessian.addDense( 4*N, 0, Hx );
if( na > 0 ) hessian.addDense( 4*N, N, Hxa );
if( np > 0 ) hessian.addDense( 4*N, 2*N, Hp );
if( nu > 0 ) hessian.addDense( 4*N, 3*N, Hu );
if( nw > 0 ) hessian.addDense( 4*N, 4*N, Hw );
}
Hx.init ( nw, nx );
Hxa.init( nw, na );
Hp.init ( nw, np );
Hu.init ( nw, nu );
Hw.init ( nw, nw );
for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed2[y_index[1][run2]] = 1.0;
fcn[1].AD_forward( 0, fseed2, R );
for( run1 = 0; run1 < nc; run1++ )
Dw( run1, run2-nx-na-np-nu ) = R[run1];
fseed2[y_index[1][run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn[1].getNumberOfVariables(); run1++ ){
J2[run1] = 0.0;
H2[run1] = 0.0;
}
fcn[1].AD_backward2( 0, bseed1, bseed2, J2, H2 );
for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np-nu, run1 ) = -H2[y_index[1][run1]];
for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx ) = -H2[y_index[1][run1]];
for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np ) = -H2[y_index[1][run1]];
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np-nu, run1-nx-na-np-nu ) = -H2[y_index[1][run1]];
}
if( nw > 0 ){
dBackward.setDense( 0, 5*N-1, Dw );
if( nx > 0 ) hessian.addDense( 5*N-1, N-1, Hx );
if( na > 0 ) hessian.addDense( 5*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian.addDense( 5*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian.addDense( 5*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian.addDense( 5*N-1, 5*N-1, Hw );
}
delete[] bseed1;
delete[] bseed2;
delete[] R ;
delete[] J1 ;
delete[] H1 ;
delete[] fseed1;
delete[] J2 ;
delete[] H2 ;
delete[] fseed2;
return SUCCESSFUL_RETURN;
}
returnValue BoundaryConstraint::evaluateSensitivities( ){
int run1;
const int N = grid.getNumPoints();
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
if( bSeed != 0 )
{
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
int nBDirs = bSeed->getNumRows( 0, 0 );
DMatrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
DMatrix Dx ( nBDirs, nx );
DMatrix Dxa( nBDirs, na );
DMatrix Dp ( nBDirs, np );
DMatrix Du ( nBDirs, nu );
DMatrix Dw ( nBDirs, nw );
// evaluate at start
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[0].AD_backward( bseed_.getRow(run1), JJ[0], 0 ) );
if( nx > 0 ) Dx .setRow( run1, JJ[0].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[0].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[0].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[0].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[0].getW () );
JJ[0].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, 0 , Dx );
if( na > 0 )
dBackward.setDense( 0, N, Dxa );
if( np > 0 )
dBackward.setDense( 0, 2*N, Dp );
if( nu > 0 )
dBackward.setDense( 0, 3*N, Du );
if( nw > 0 )
dBackward.setDense( 0, 4*N, Dw );
// evaluate at end
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[1].AD_backward( bseed_.getRow(run1), JJ[1], 0 ) );
if( nx > 0 ) Dx .setRow( run1, JJ[1].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[1].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[1].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[1].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[1].getW () );
JJ[1].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, N-1, Dx );
if( na > 0 )
dBackward.setDense( 0, 2*N-1, Dxa );
if( np > 0 )
dBackward.setDense( 0, 3*N-1, Dp );
if( nu > 0 )
dBackward.setDense( 0, 4*N-1, Du );
if( nw > 0 )
dBackward.setDense( 0, 5*N-1, Dw );
return SUCCESSFUL_RETURN;
}
// TODO: implement forward mode
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
returnValue MayerTerm::evaluateSensitivities( BlockMatrix *hessian ){
int run1, run2;
const int N = grid.getNumPoints();
if( (bSeed != 0) & (hessian == 0) ){
Matrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
fcn.AD_backward( bseed_.getRow(0), JJ );
dBackward.init( 1, 5*N );
if( nx > 0 ) dBackward.setDense( 0, N-1, Matrix( JJ.getX (), BT_TRUE ) );
if( na > 0 ) dBackward.setDense( 0, 2*N-1, Matrix( JJ.getXA(), BT_TRUE ) );
if( np > 0 ) dBackward.setDense( 0, 3*N-1, Matrix( JJ.getP (), BT_TRUE ) );
if( nu > 0 ) dBackward.setDense( 0, 4*N-1, Matrix( JJ.getU (), BT_TRUE ) );
if( nw > 0 ) dBackward.setDense( 0, 5*N-1, Matrix( JJ.getW (), BT_TRUE ) );
return SUCCESSFUL_RETURN;
}
if( hessian != 0 ){
double bseed1 = 1.0;
double bseed2 = 0.0;
double *J = new double[fcn.getNumberOfVariables() +1];
double *H = new double[fcn.getNumberOfVariables() +1];
double *fseed = new double[fcn.getNumberOfVariables() +1];
for( run1 = 0; run1 < fcn.getNumberOfVariables()+1; run1++ )
fseed[run1] = 0.0;
dBackward.init( 1, 5*N );
Matrix Dx ( 1, nx );
Matrix Dxa( 1, na );
Matrix Dp ( 1, np );
Matrix Du ( 1, nu );
Matrix Dw ( 1, nw );
Matrix Hx ( nx, nx );
Matrix Hxa( nx, na );
Matrix Hp ( nx, np );
Matrix Hu ( nx, nu );
Matrix Hw ( nx, nw );
for( run2 = 0; run2 < nx; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[run2]] = 1.0;
fcn.AD_forward( 0, fseed, J );
Dx( 0, run2 ) = J[0];
fseed[y_index[run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
for( run1 = 0; run1 < nx; run1++ ){
Hx( run2, run1 ) = H[y_index[run1]];
}
for( run1 = nx; run1 < nx+na; run1++ ){
Hxa( run2, run1-nx ) = H[y_index[run1]];
}
for( run1 = nx+na; run1 < nx+na+np; run1++ ){
Hp( run2, run1-nx-na ) = H[y_index[run1]];
}
for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
Hu( run2, run1-nx-na-np ) = H[y_index[run1]];
}
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
Hw( run2, run1-nx-na-np-nu ) = H[y_index[run1]];
}
}
if( nx > 0 ){
dBackward.setDense( 0, N-1, Dx );
if( nx > 0 ) hessian->setDense( N-1, N-1, Hx );
if( na > 0 ) hessian->setDense( N-1, 2*N-1, Hxa );
if( np > 0 ) hessian->setDense( N-1, 3*N-1, Hp );
if( nu > 0 ) hessian->setDense( N-1, 4*N-1, Hu );
if( nw > 0 ) hessian->setDense( N-1, 5*N-1, Hw );
}
Hx.init ( na, nx );
Hxa.init( na, na );
Hp.init ( na, np );
Hu.init ( na, nu );
Hw.init ( na, nw );
for( run2 = nx; run2 < nx+na; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[run2]] = 1.0;
fcn.AD_forward( 0, fseed, J );
Dxa( 0, run2-nx ) = J[0];
fseed[y_index[run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
for( run1 = 0; run1 < nx; run1++ ){
Hx( run2-nx, run1 ) = H[y_index[run1]];
}
for( run1 = nx; run1 < nx+na; run1++ ){
Hxa( run2-nx, run1-nx ) = H[y_index[run1]];
}
for( run1 = nx+na; run1 < nx+na+np; run1++ ){
Hp( run2-nx, run1-nx-na ) = H[y_index[run1]];
}
for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
Hu( run2-nx, run1-nx-na-np ) = H[y_index[run1]];
}
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
Hw( run2-nx, run1-nx-na-np-nu ) = H[y_index[run1]];
}
}
if( na > 0 ){
dBackward.setDense( 0, 2*N-1, Dxa );
if( nx > 0 ) hessian->setDense( 2*N-1, N-1, Hx );
if( na > 0 ) hessian->setDense( 2*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian->setDense( 2*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian->setDense( 2*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian->setDense( 2*N-1, 5*N-1, Hw );
}
Hx.init ( np, nx );
Hxa.init( np, na );
Hp.init ( np, np );
Hu.init ( np, nu );
Hw.init ( np, nw );
for( run2 = nx+na; run2 < nx+na+np; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[run2]] = 1.0;
fcn.AD_forward( 0, fseed, J );
Dp( 0, run2-nx-na ) = J[0];
fseed[y_index[run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
for( run1 = 0; run1 < nx; run1++ ){
Hx( run2-nx-na, run1 ) = H[y_index[run1]];
}
for( run1 = nx; run1 < nx+na; run1++ ){
Hxa( run2-nx-na, run1-nx ) = H[y_index[run1]];
}
for( run1 = nx+na; run1 < nx+na+np; run1++ ){
Hp( run2-nx-na, run1-nx-na ) = H[y_index[run1]];
}
for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
Hu( run2-nx-na, run1-nx-na-np ) = H[y_index[run1]];
}
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
Hw( run2-nx-na, run1-nx-na-np-nu ) = H[y_index[run1]];
}
}
if( np > 0 ){
dBackward.setDense( 0, 3*N-1, Dp );
if( nx > 0 ) hessian->setDense( 3*N-1, N-1, Hx );
if( na > 0 ) hessian->setDense( 3*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian->setDense( 3*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian->setDense( 3*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian->setDense( 3*N-1, 5*N-1, Hw );
}
Hx.init ( nu, nx );
Hxa.init( nu, na );
Hp.init ( nu, np );
Hu.init ( nu, nu );
Hw.init ( nu, nw );
for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[run2]] = 1.0;
fcn.AD_forward( 0, fseed, J );
Du( 0, run2-nx-na-np ) = J[0];
fseed[y_index[run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
for( run1 = 0; run1 < nx; run1++ ){
Hx( run2-nx-na-np, run1 ) = H[y_index[run1]];
}
for( run1 = nx; run1 < nx+na; run1++ ){
Hxa( run2-nx-na-np, run1-nx ) = H[y_index[run1]];
}
for( run1 = nx+na; run1 < nx+na+np; run1++ ){
Hp( run2-nx-na-np, run1-nx-na ) = H[y_index[run1]];
}
for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
Hu( run2-nx-na-np, run1-nx-na-np ) = H[y_index[run1]];
}
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
Hw( run2-nx-na-np, run1-nx-na-np-nu ) = H[y_index[run1]];
}
}
if( nu > 0 ){
dBackward.setDense( 0, 4*N-1, Du );
if( nx > 0 ) hessian->setDense( 4*N-1, N-1, Hx );
if( na > 0 ) hessian->setDense( 4*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian->setDense( 4*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian->setDense( 4*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian->setDense( 4*N-1, 5*N-1, Hw );
}
Hx.init ( nw, nx );
Hxa.init( nw, na );
Hp.init ( nw, np );
Hu.init ( nw, nu );
Hw.init ( nw, nw );
for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
// FIRST ORDER DERIVATIVES:
// ------------------------
fseed[y_index[run2]] = 1.0;
fcn.AD_forward( 0, fseed, J );
Dw( 0, run2-nx-na-np-nu ) = J[0];
fseed[y_index[run2]] = 0.0;
// SECOND ORDER DERIVATIVES:
// -------------------------
for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
J[run1] = 0.0;
H[run1] = 0.0;
}
fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
for( run1 = 0; run1 < nx; run1++ ){
Hx( run2-nx-na-np-nu, run1 ) = H[y_index[run1]];
}
for( run1 = nx; run1 < nx+na; run1++ ){
Hxa( run2-nx-na-np-nu, run1-nx ) = H[y_index[run1]];
}
for( run1 = nx+na; run1 < nx+na+np; run1++ ){
Hp( run2-nx-na-np-nu, run1-nx-na ) = H[y_index[run1]];
}
for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
Hu( run2-nx-na-np-nu, run1-nx-na-np ) = H[y_index[run1]];
}
for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
Hw( run2-nx-na-np-nu, run1-nx-na-np-nu ) = H[y_index[run1]];
}
}
if( nw > 0 ){
dBackward.setDense( 0, 5*N-1, Dw );
if( nx > 0 ) hessian->setDense( 5*N-1, N-1, Hx );
if( na > 0 ) hessian->setDense( 5*N-1, 2*N-1, Hxa );
if( np > 0 ) hessian->setDense( 5*N-1, 3*N-1, Hp );
if( nu > 0 ) hessian->setDense( 5*N-1, 4*N-1, Hu );
if( nw > 0 ) hessian->setDense( 5*N-1, 5*N-1, Hw );
}
delete[] J;
delete[] H;
delete[] fseed;
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
