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 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 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);
}
