/**
* copy constructor, make sure the input matrix and all the blocks have been allocated and filled before the copying
* @param blockmat_copy The blockmatrix you want to be copied into the object you are constructing
*/
BlockMatrix::BlockMatrix(const BlockMatrix &blockmat_copy){
this->nr = blockmat_copy.nr;
blockmatrix = new Matrix * [nr];
dim = new int [nr];
flag = new int [nr];
degen = new int [nr];
for(int i = 0;i < nr;++i){
flag[i] = 1;
degen[i] = blockmat_copy.gdeg(i);
dim[i] = blockmat_copy.gdim(i);
blockmatrix[i] = new Matrix(blockmat_copy[i]);
}
}
returnValue DynamicDiscretization::getResiduum( BlockMatrix &residuum_ ) const{
int run1;
uint run2;
residuum_.init( N, 1 );
for( run1 = 0; run1 < N; run1++ ){
Matrix tmp( residuum.getNumValues(), 1 );
for( run2 = 0; run2 < residuum.getNumValues(); run2++ )
tmp( run2, 0 ) = residuum(run1,run2);
residuum_.setDense(run1,0,tmp);
}
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::getAnySensitivities( BlockMatrix& _sens,
uint idx
) const
{
if ( idx > 4 )
return ACADOERROR( RET_INVALID_ARGUMENTS );
uint N = bandedCP.dynGradient.getNumRows();
Matrix tmp;
_sens.init( N,1 );
for( uint i=0; i<N; ++i )
{
bandedCP.dynGradient.getSubBlock( i,idx,tmp );
_sens.setDense( i,0,tmp );
}
return SUCCESSFUL_RETURN;
}
BDM::DistMatrix( const BlockMatrix<T>& A )
: BCM(A.Grid())
{
EL_DEBUG_CSE
if( COLDIST == CIRC && ROWDIST == CIRC )
this->matrix_.SetViewType( OWNER );
this->SetShifts();
#define GUARD(CDIST,RDIST,WRAP) \
A.DistData().colDist == CDIST && A.DistData().rowDist == RDIST && \
A.Wrap() == WRAP
#define PAYLOAD(CDIST,RDIST,WRAP) \
auto& ACast = \
static_cast<const DistMatrix<T,CDIST,RDIST,BLOCK>&>(A); \
if( COLDIST != CDIST || ROWDIST != RDIST || BLOCK != WRAP || \
reinterpret_cast<const BDM*>(&A) != this ) \
*this = ACast; \
else \
LogicError("Tried to construct DistMatrix with itself");
#include "El/macros/GuardAndPayload.h"
}
returnValue ConstantHessian::initHessian( BlockMatrix& B,
uint N,
const OCPiterate& iter
)
{
if( N > 1 )
{
for( uint run1=0; run1<N; ++run1 )
{
if ( iter.getNX() != 0 )
B.setIdentity( run1,run1, iter.getNX() );
if ( iter.getNXA() != 0 )
B.setIdentity( N+run1,N+run1, iter.getNXA() );
if ( ( iter.getNP() != 0 ) && ( run1 != N-1 ) )
B.setIdentity( 2*N+run1,2*N+run1, iter.getNP() );
if ( ( iter.getNU() != 0 ) && ( run1 != N-1 ) )
B.setIdentity( 3*N+run1,3*N+run1, iter.getNU() );
if ( ( iter.getNW() != 0 ) && ( run1 != N-1 ) )
B.setIdentity( 4*N+run1,4*N+run1, iter.getNW() );
}
}
else
{
if ( iter.getNP() != 0 )
B.setIdentity( 2,2, iter.getNP() );
if ( iter.getNU() != 0 )
B.setIdentity( 3,3, iter.getNU() );
if ( iter.getNW() != 0 )
B.setIdentity( 4,4, iter.getNW() );
}
return SUCCESSFUL_RETURN;
}
returnValue Objective::evaluateSensitivitiesGN( BlockMatrix &hessian ){
returnValue returnvalue;
uint run1;
hessian.setZero();
if( nMayer != 0 )
return ACADOERROR(RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED);
for( run1 = 0; run1 < nLSQ; run1++ ){
returnvalue = lsqTerm[run1]->evaluateSensitivitiesGN( &hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
}
for( run1 = 0; run1 < nEndLSQ; run1++ ){
returnvalue = lsqEndTerm[run1]->evaluateSensitivitiesGN( &hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
}
return SUCCESSFUL_RETURN;
}
returnValue Constraint::getBoundResiduum( BlockMatrix &lowerRes,
BlockMatrix &upperRes ) {
int run1;
const int N = grid.getNumPoints();
lowerRes.init( 4*N+1, 1 );
upperRes.init( 4*N+1, 1 );
for( run1 = 0; run1 < N; run1++ ) {
lowerRes.setDense( run1, 0, residuumXL[run1] );
upperRes.setDense( run1, 0, residuumXU[run1] );
lowerRes.setDense( N+run1, 0, residuumXAL[run1] );
upperRes.setDense( N+run1, 0, residuumXAU[run1] );
lowerRes.setDense( 2*N+1+run1, 0, residuumUL[run1] );
upperRes.setDense( 2*N+1+run1, 0, residuumUU[run1] );
lowerRes.setDense( 3*N+1+run1, 0, residuumWL[run1] );
upperRes.setDense( 3*N+1+run1, 0, residuumWU[run1] );
}
lowerRes.setDense( 2*N, 0, residuumPL[0] );
upperRes.setDense( 2*N, 0, residuumPU[0] );
return SUCCESSFUL_RETURN;
}
returnValue Constraint::getForwardSensitivities( BlockMatrix &D, int order ) {
const int N = grid.getNumPoints();
returnValue returnvalue;
BlockMatrix result;
result.init( getNumberOfBlocks(), 5*N );
int nc, run1, run2;
nc = 0;
// BOUNDARY CONSTRAINTS:
// ---------------------
if( boundary_constraint->getNC() != 0 ) {
BlockMatrix res;
returnvalue = boundary_constraint->getForwardSensitivities( &res, order );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix res_;
for( run2 = 0; run2 < 5*N; run2++ ) {
res.getSubBlock( 0 , run2, res_ );
if( res_.getDim() > 0 )
result.setDense( nc, run2, res_ );
}
nc++;
}
// COUPLED PATH CONSTRAINTS:
// -------------------------
if( coupled_path_constraint->getNC() != 0 ) {
BlockMatrix res;
returnvalue = coupled_path_constraint->getForwardSensitivities( &res, order );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix res_;
for( run2 = 0; run2 < 5*N; run2++ ) {
res.getSubBlock( 0 , run2, res_ );
if( res_.getDim() > 0 )
result.setDense( nc, run2, res_ );
}
nc++;
}
// PATH CONSTRAINTS:
// -----------------
if( path_constraint->getNC() != 0 ) {
BlockMatrix res;
returnvalue = path_constraint->getForwardSensitivities( &res, order );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix res_;
for( run1 = 0; run1 < N; run1++ ) {
for( run2 = 0; run2 < 5*N; run2++ ) {
res.getSubBlock( run1, run2, res_ );
if( res_.getDim() > 0 )
result.setDense( nc , run2, res_ );
}
nc++;
}
}
// ALGEBRAIC CONSISTENCY CONSTRAINTS:
// ----------------------------------
if( algebraic_consistency_constraint->getNC() != 0 ) {
BlockMatrix res;
returnvalue = algebraic_consistency_constraint->getForwardSensitivities( &res, order );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix res_;
for( run1 = 0; run1 < N; run1++ ) {
for( run2 = 0; run2 < 5*N; run2++ ) {
res.getSubBlock( run1, run2, res_ );
if( res_.getDim() > 0 )
result.setDense( nc , run2, res_ );
}
nc++;
}
}
// POINT CONSTRAINTS:
// ------------------
if( point_constraints != 0 ) {
for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ) {
if( point_constraints[run1] != 0 ) {
BlockMatrix res;
returnvalue = point_constraints[run1]->getForwardSensitivities( &res, order );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix res_;
for( run2 = 0; run2 < 5*N; run2++ ) {
res.getSubBlock( 0 , run2, res_ );
if( res_.getDim() > 0 )
result.setDense( nc, run2, res_ );
}
nc++;
}
}
}
D = result;
return SUCCESSFUL_RETURN;
}
returnValue Constraint::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ) {
uint run1 ;
int count;
returnValue returnvalue;
count = 0;
Matrix tmp;
// EVALUATE BOUNDARY CONSTRAINS:
// -----------------------------
if( boundary_constraint->getNC() != 0 ) {
seed.getSubBlock( count, 0, tmp, boundary_constraint->getNC(), 1 );
returnvalue = boundary_constraint->evaluateSensitivities( tmp, hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
count++;
}
// EVALUATE COUPLED PATH CONSTRAINS:
// ---------------------------------
if( coupled_path_constraint->getNC() != 0 ) {
seed.getSubBlock( count, 0, tmp, coupled_path_constraint->getNC(), 1 );
returnvalue = coupled_path_constraint->evaluateSensitivities( tmp, hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
count++;
}
// EVALUATE PATH CONSTRAINS:
// -------------------------
if( path_constraint->getNC() != 0 ) {
returnvalue = path_constraint->evaluateSensitivities( count, seed, hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
// EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
// ------------------------------------------
if( algebraic_consistency_constraint->getNC() != 0 ) {
returnvalue = algebraic_consistency_constraint->evaluateSensitivities( count, seed, hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
// EVALUATE POINT CONSTRAINS:
// --------------------------
if( point_constraints != 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( point_constraints[run1] != 0 ) {
seed.getSubBlock( count, 0, tmp, point_constraints[run1]->getNC(), 1 );
returnvalue = point_constraints[run1]->evaluateSensitivities( tmp, hessian );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
count++;
}
}
}
return SUCCESSFUL_RETURN;
}
returnValue Constraint::getConstraintResiduum( BlockMatrix &lowerRes, BlockMatrix &upperRes ) {
const int N = grid.getNumPoints();
returnValue returnvalue;
BlockMatrix residuumL;
BlockMatrix residuumU;
residuumL.init( getNumberOfBlocks(), 1 );
residuumU.init( getNumberOfBlocks(), 1 );
int nc, run1;
nc = 0;
// BOUNDARY CONSTRAINTS:
// ---------------------
if( boundary_constraint->getNC() != 0 ) {
BlockMatrix resL, resU;
returnvalue = boundary_constraint->getResiduum( resL, resU );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix resL_;
Matrix resU_;
resL.getSubBlock( 0, 0, resL_ );
resU.getSubBlock( 0, 0, resU_ );
residuumL.setDense( nc, 0, resL_ );
residuumU.setDense( nc, 0, resU_ );
nc++;
}
// COUPLED PATH CONSTRAINTS:
// -------------------------
if( coupled_path_constraint->getNC() != 0 ) {
BlockMatrix resL, resU;
returnvalue = coupled_path_constraint->getResiduum( resL, resU );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix resL_;
Matrix resU_;
resL.getSubBlock( 0, 0, resL_ );
resU.getSubBlock( 0, 0, resU_ );
residuumL.setDense( nc, 0, resL_ );
residuumU.setDense( nc, 0, resU_ );
nc++;
}
// PATH CONSTRAINTS:
// -----------------
if( path_constraint->getNC() != 0 ) {
BlockMatrix resL, resU;
returnvalue = path_constraint->getResiduum( resL, resU );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix resL_;
Matrix resU_;
for( run1 = 0; run1 < N; run1++ ) {
resL.getSubBlock( run1, 0, resL_ );
resU.getSubBlock( run1, 0, resU_ );
residuumL.setDense( nc, 0, resL_ );
residuumU.setDense( nc, 0, resU_ );
nc++;
}
}
// ALGEBRAIC CONSISTENCY CONSTRAINTS:
// ----------------------------------
if( algebraic_consistency_constraint->getNC() != 0 ) {
BlockMatrix resL, resU;
returnvalue = algebraic_consistency_constraint->getResiduum( resL, resU );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix resL_;
Matrix resU_;
for( run1 = 0; run1 < N; run1++ ) {
resL.getSubBlock( run1, 0, resL_ );
resU.getSubBlock( run1, 0, resU_ );
residuumL.setDense( nc, 0, resL_ );
residuumU.setDense( nc, 0, resU_ );
nc++;
}
}
// POINT CONSTRAINTS:
// ------------------
if( point_constraints != 0 ) {
for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ) {
if( point_constraints[run1] != 0 ) {
BlockMatrix resL, resU;
returnvalue = point_constraints[run1]->getResiduum( resL, resU );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
Matrix resL_;
Matrix resU_;
resL.getSubBlock( 0, 0, resL_ );
resU.getSubBlock( 0, 0, resU_ );
residuumL.setDense( nc, 0, resL_ );
residuumU.setDense( nc, 0, resU_ );
nc++;
}
}
}
lowerRes = residuumL;
upperRes = residuumU;
return SUCCESSFUL_RETURN;
}
void EntrywiseMap
( const BlockMatrix<S>& A,
BlockMatrix<T>& B,
function<T(S)> func )
{
if( A.DistData().colDist == B.DistData().colDist &&
A.DistData().rowDist == B.DistData().rowDist )
{
B.AlignWith( A.DistData() );
B.Resize( A.Height(), A.Width() );
EntrywiseMap( A.LockedMatrix(), B.Matrix(), func );
}
else
{
B.Resize( A.Height(), A.Width() );
#define GUARD(CDIST,RDIST) \
B.DistData().colDist == CDIST && B.DistData().rowDist == RDIST
#define PAYLOAD(CDIST,RDIST) \
DistMatrix<S,CDIST,RDIST,BLOCK> AProx(B.Grid()); \
AProx.AlignWith( B.DistData() ); \
Copy( A, AProx ); \
EntrywiseMap( AProx.Matrix(), B.Matrix(), func );
#include <El/macros/GuardAndPayload.h>
#undef GUARD
#undef PAYLOAD
}
}
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;
}
void TransposeContract
( const BlockMatrix<T>& A,
BlockMatrix<T>& B, bool conjugate )
{
EL_DEBUG_CSE
const Dist U = B.ColDist();
const Dist V = B.RowDist();
if( A.ColDist() == V && A.RowDist() == Partial(U) )
{
Transpose( A, B, conjugate );
}
else
{
unique_ptr<BlockMatrix<T>>
ASumFilt( B.ConstructTranspose(B.Grid(),B.Root()) );
if( B.ColConstrained() )
ASumFilt->AlignRowsWith( B, true );
if( B.RowConstrained() )
ASumFilt->AlignColsWith( B, true );
Contract( A, *ASumFilt );
if( !B.ColConstrained() )
B.AlignColsWith( *ASumFilt, false );
if( !B.RowConstrained() )
B.AlignRowsWith( *ASumFilt, false );
// We should have ensured that the alignments match
B.Resize( A.Width(), A.Height() );
Transpose( ASumFilt->LockedMatrix(), B.Matrix(), conjugate );
}
}
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;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// DEFINE SOME MATRICES:
// ---------------------
Matrix A(2,2), B(2,3), C(2,2);
A(0,0) = 1.0; A(0,1) = 2.0;
A(1,0) = 3.0; A(1,1) = 4.0;
B(0,0) = 1.0; B(0,1) = 2.0; B(0,2) = 3.0;
B(1,0) = 4.0; B(1,1) = 5.0; B(1,2) = 6.0;
C(0,0) = 1.0; C(0,1) = 2.0;
C(1,0) = 4.0; C(1,1) = 5.0;
// DEFINE SOME BLOCK MATRICES:
// ---------------------------
BlockMatrix M(2,2),N(2,3),P(2,3);
// -------------------------------------
// DEFINE A BLOCK MATRIX M OF THE FORM:
//
// ( 1 A )
// M := ( )
// ( 0 1 )
//
// WHERE 1 IS A 2x2 UNIT MATRIX:
// -------------------------------------
M.setIdentity(0,0,2); M.setDense (0,1,A);
/* skip */ M.setIdentity(1,1,2);
// -------------------------------------
// DEFINE A BLOCK MATRIX N OF THE FORM:
//
// ( 1 B C )
// N := ( )
// ( 0 B 1 )
//
// -------------------------------------
N.setIdentity(0,0,2); N.setDense(0,1,B); N.setDense (0,2,C);
/* skip */ N.setDense(1,1,B); N.setIdentity(1,2,2);
// PRINT THE MATRICES M AND N :
// -------------------------------
printf("M = \n"); M.print();
printf("N = \n"); N.print();
// COMPUTE THE MATRIX PRODUCT MN := M*N :
// ---------------------------------------
BlockMatrix MN; MN = M*N;
// PRINT THE RESULT FOR MN:
// ------------------------
printf("MN = \n"); MN.print();
// COMPUTE THE MATRIX PRODUCT MTN := M^T*N :
// ------------------------------------------
BlockMatrix MTN; MTN = M^N;
// PRINT THE RESULT FOR MN:
// ------------------------
printf("MTN = \n"); MTN.print();
return 0;
}
void IndexDependentMap
( const BlockMatrix<S>& A,
BlockMatrix<T>& B,
function<T(Int,Int,S)> func )
{
DEBUG_CSE
const Int mLoc = A.LocalHeight();
const Int nLoc = A.LocalWidth();
B.AlignWith( A.DistData() );
B.Resize( A.Height(), A.Width() );
auto& ALoc = A.LockedMatrix();
auto& BLoc = B.Matrix();
for( Int jLoc=0; jLoc<nLoc; ++jLoc )
{
const Int j = A.GlobalCol(jLoc);
for( Int iLoc=0; iLoc<mLoc; ++iLoc )
{
const Int i = A.GlobalRow(iLoc);
BLoc(iLoc,jLoc) = func(i,j,ALoc(iLoc,jLoc));
}
}
}
void Contract
( const BlockMatrix<T>& A,
BlockMatrix<T>& B )
{
DEBUG_ONLY(CSE cse("Contract"))
AssertSameGrids( A, B );
const Dist U = B.ColDist();
const Dist V = B.RowDist();
// TODO: Shorten this implementation?
if( A.ColDist() == U && A.RowDist() == V )
{
Copy( A, B );
}
else if( A.ColDist() == U && A.RowDist() == Partial(V) )
{
B.AlignAndResize
( A.BlockHeight(), A.BlockWidth(),
A.ColAlign(), A.RowAlign(), A.ColCut(), A.RowCut(),
A.Height(), A.Width(), false, false );
Zeros( B.Matrix(), B.LocalHeight(), B.LocalWidth() );
AxpyContract( T(1), A, B );
}
else if( A.ColDist() == Partial(U) && A.RowDist() == V )
{
B.AlignAndResize
( A.BlockHeight(), A.BlockWidth(),
A.ColAlign(), A.RowAlign(), A.ColCut(), A.RowCut(),
A.Height(), A.Width(), false, false );
Zeros( B.Matrix(), B.LocalHeight(), B.LocalWidth() );
AxpyContract( T(1), A, B );
}
else if( A.ColDist() == U && A.RowDist() == Collect(V) )
{
B.AlignColsAndResize
( A.BlockHeight(), A.ColAlign(), A.ColCut(), A.Height(), A.Width(),
false, false );
Zeros( B.Matrix(), B.LocalHeight(), B.LocalWidth() );
AxpyContract( T(1), A, B );
}
else if( A.ColDist() == Collect(U) && A.RowDist() == V )
{
B.AlignRowsAndResize
( A.BlockWidth(), A.RowAlign(), A.RowCut(), A.Height(), A.Width(),
false, false );
Zeros( B.Matrix(), B.LocalHeight(), B.LocalWidth() );
AxpyContract( T(1), A, B );
}
else if( A.ColDist() == Collect(U) && A.RowDist() == Collect(V) )
{
Zeros( B, A.Height(), A.Width() );
AxpyContract( T(1), A, B );
}
else
LogicError("Incompatible distributions");
}
void Gather
( const BlockMatrix<T>& A,
DistMatrix<T,CIRC,CIRC,BLOCK>& B )
{
DEBUG_ONLY(CSE cse("copy::Gather"))
AssertSameGrids( A, B );
if( A.DistSize() == 1 && A.CrossSize() == 1 )
{
B.Resize( A.Height(), A.Width() );
if( B.CrossRank() == B.Root() )
Copy( A.LockedMatrix(), B.Matrix() );
return;
}
const Int height = A.Height();
const Int width = A.Width();
B.SetGrid( A.Grid() );
B.Resize( height, width );
// Gather the colShifts and rowShifts
// ==================================
Int myShifts[2];
myShifts[0] = A.ColShift();
myShifts[1] = A.RowShift();
vector<Int> shifts;
const Int crossSize = B.CrossSize();
if( B.CrossRank() == B.Root() )
shifts.resize( 2*crossSize );
mpi::Gather( myShifts, 2, shifts.data(), 2, B.Root(), B.CrossComm() );
// Gather the payload data
// =======================
const bool irrelevant = ( A.RedundantRank()!=0 || A.CrossRank()!=A.Root() );
int totalSend = ( irrelevant ? 0 : A.LocalHeight()*A.LocalWidth() );
vector<int> recvCounts, recvOffsets;
if( B.CrossRank() == B.Root() )
recvCounts.resize( crossSize );
mpi::Gather( &totalSend, 1, recvCounts.data(), 1, B.Root(), B.CrossComm() );
int totalRecv = Scan( recvCounts, recvOffsets );
//vector<T> sendBuf(totalSend), recvBuf(totalRecv);
vector<T> sendBuf, recvBuf;
sendBuf.reserve( totalSend );
recvBuf.reserve( totalRecv );
if( !irrelevant )
copy::util::InterleaveMatrix
( A.LocalHeight(), A.LocalWidth(),
A.LockedBuffer(), 1, A.LDim(),
sendBuf.data(), 1, A.LocalHeight() );
mpi::Gather
( sendBuf.data(), totalSend,
recvBuf.data(), recvCounts.data(), recvOffsets.data(),
B.Root(), B.CrossComm() );
// Unpack
// ======
const Int mb = A.BlockHeight();
const Int nb = A.BlockWidth();
const Int colCut = A.ColCut();
const Int rowCut = A.RowCut();
if( B.Root() == B.CrossRank() )
{
for( Int q=0; q<crossSize; ++q )
{
if( recvCounts[q] == 0 )
continue;
const Int colShift = shifts[2*q+0];
const Int rowShift = shifts[2*q+1];
const Int colStride = A.ColStride();
const Int rowStride = A.RowStride();
const Int localHeight =
BlockedLength( height, colShift, mb, colCut, colStride );
const Int localWidth =
BlockedLength( width, rowShift, nb, rowCut, rowStride );
const T* data = &recvBuf[recvOffsets[q]];
for( Int jLoc=0; jLoc<localWidth; ++jLoc )
{
const Int jBefore = rowShift*nb - rowCut;
const Int jLocAdj = ( rowShift==0 ? jLoc+rowCut : jLoc );
const Int numFilledLocalBlocks = jLocAdj / nb;
const Int jMid = numFilledLocalBlocks*nb*rowStride;
const Int jPost = jLocAdj-numFilledLocalBlocks*nb;
const Int j = jBefore + jMid + jPost;
const T* sourceCol = &data[jLoc*localHeight];
for( Int iLoc=0; iLoc<localHeight; ++iLoc )
{
const Int iBefore = colShift*mb - colCut;
const Int iLocAdj = (colShift==0 ? iLoc+colCut : iLoc);
const Int numFilledLocalBlocks = iLocAdj / mb;
const Int iMid = numFilledLocalBlocks*mb*colStride;
const Int iPost = iLocAdj-numFilledLocalBlocks*mb;
const Int i = iBefore + iMid + iPost;
B.SetLocal(i,j,sourceCol[iLoc]);
}
}
}
}
}
returnValue ShootingMethod::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ){
const int NN = N+1;
dForward.init( N, 5 );
int i;
for( i = 0; i < N; i++ ){
Matrix X, P, U, W, D, E, HX, HP, HU, HW, S;
if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
seed.getSubBlock( i, 0, S, nx, 1 );
if( nx > 0 ){
ACADO_TRY( differentiateForwardBackward( i, X, E, E, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 0, D );
if( nx > 0 ) hessian.addDense( i, i, HX );
if( np > 0 ) hessian.addDense( i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( i, 4*NN+i, HW );
}
if( np > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, P, E, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 2, D );
if( nx > 0 ) hessian.addDense( 2*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 2*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 2*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 2*NN+i, 4*NN+i, HW );
}
if( nu > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, E, U, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 3, D );
if( nx > 0 ) hessian.addDense( 3*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 3*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 3*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 3*NN+i, 4*NN+i, HW );
}
if( nw > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, E, E, W, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 4, D );
if( nx > 0 ) hessian.addDense( 4*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 4*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 4*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 4*NN+i, 4*NN+i, HW );
}
}
return SUCCESSFUL_RETURN;
}
