00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00034 #include <acado/code_generation/export_variable.hpp>
00035 #include <acado/code_generation/export_variable_internal.hpp>
00036
00037 BEGIN_NAMESPACE_ACADO
00038
00039 using namespace std;
00040
00041 static const double undefinedEntry = 1073741824.03125;
00042
00043
00044
00045
00046
00047
00048 ExportVariableInternal::ExportVariableInternal( ) : ExportArgumentInternal( )
00049 {
00050 doAccessTransposed = false;
00051
00052 rowOffset = 0;
00053 colOffset = 0;
00054
00055 rowDim = 0;
00056 colDim = 0;
00057 nRows = 0;
00058 nCols = 0;
00059 }
00060
00061 ExportVariableInternal::ExportVariableInternal( const std::string& _name,
00062 const DMatrixPtr& _data,
00063 ExportType _type,
00064 ExportStruct _dataStruct,
00065 bool _callItByValue,
00066 const std::string& _prefix
00067 )
00068 : ExportArgumentInternal(_name, _data, _type, _dataStruct, _callItByValue, emptyConstExportIndex, _prefix)
00069 {
00070 doAccessTransposed = false;
00071
00072 rowOffset = 0;
00073 colOffset = 0;
00074
00075 rowDim = _data->getNumRows();
00076 colDim = _data->getNumCols();
00077 nRows = 0;
00078 nCols = 0;
00079 }
00080
00081
00082 ExportVariableInternal::~ExportVariableInternal( )
00083 {}
00084
00085
00086 ExportVariableInternal* ExportVariableInternal::clone() const
00087 {
00088 return new ExportVariableInternal( *this );
00089 }
00090
00091 bool ExportVariableInternal::isZero( const ExportIndex& rowIdx,
00092 const ExportIndex& colIdx
00093 ) const
00094 {
00095 return hasValue(rowIdx, colIdx, 0.0);
00096 }
00097
00098 bool ExportVariableInternal::isOne( const ExportIndex& rowIdx,
00099 const ExportIndex& colIdx
00100 ) const
00101 {
00102 return hasValue(rowIdx, colIdx, 1.0);
00103 }
00104
00105
00106 bool ExportVariableInternal::isGiven( const ExportIndex& rowIdx,
00107 const ExportIndex& colIdx
00108 ) const
00109 {
00110 if (hasValue(rowIdx, colIdx, undefinedEntry) == true)
00111 return false;
00112
00113 return true;
00114 }
00115
00116 bool ExportVariableInternal::isGiven() const
00117 {
00118 return ExportArgumentInternal::isGiven();
00119 }
00120
00121
00122 const std::string ExportVariableInternal::get( const ExportIndex& rowIdx,
00123 const ExportIndex& colIdx
00124 ) const
00125 {
00126 stringstream s;
00127
00128 IoFormatter iof( s );
00129
00130 iof.set( getType() == INT || getType() == STATIC_CONST_INT ? 5 : 16,
00131 iof.width,
00132 getType() == INT || getType() == STATIC_CONST_INT ? ios::fixed : ios::scientific
00133 );
00134
00135 ExportIndex totalIdx = getTotalIdx(rowIdx + rowOffset, colIdx + colOffset);
00136
00137 if ( ( totalIdx.isGiven() == true ) && ( rowIdx.isGiven() == true ) && ( colIdx.isGiven() == true ) )
00138 {
00139 if (isGiven(rowIdx, colIdx) == false)
00140 {
00141 if ( ( isCalledByValue() == true ) && ( totalIdx.getGivenValue() == 0 ) )
00142 s << getFullName();
00143 else
00144 s << getFullName() << "[" << totalIdx.getGivenValue() << "]";
00145 }
00146 else
00147 {
00148 s << "(real_t)" << data->operator()(totalIdx.getGivenValue());
00149 }
00150 }
00151 else
00152 s << getFullName() << "[" << totalIdx.get( ) << "]";
00153
00154 return s.str();
00155 }
00156
00157
00158 uint ExportVariableInternal::getNumRows( ) const
00159 {
00160 if ( nRows > 0 )
00161 return nRows;
00162
00163 return data->getNumRows( );
00164 }
00165
00166
00167 uint ExportVariableInternal::getNumCols( ) const
00168 {
00169 if ( nCols > 0 )
00170 return nCols;
00171
00172 return data->getNumCols( );
00173 }
00174
00175
00176 uint ExportVariableInternal::getDim( ) const
00177 {
00178 return (getNumRows() * getNumCols());
00179 }
00180
00181
00182 ExportVariable ExportVariableInternal::getTranspose( ) const
00183 {
00184 DMatrix m = data->transpose();
00185
00186 ExportVariable transposed(name, m, type, dataStruct, callItByValue, prefix);
00187 transposed->setSubmatrixOffsets(colOffset, rowOffset, colDim, rowDim, nCols, nRows);
00188 transposed->doAccessTransposed = true;
00189
00190 return transposed;
00191 }
00192
00193
00194 ExportVariable ExportVariableInternal::getRow( const ExportIndex& idx
00195 ) const
00196 {
00197 ASSERT( doAccessTransposed == false );
00198
00199 ExportVariable tmp(name, data, type, dataStruct, callItByValue, prefix);
00200
00201 if (idx.isGiven() == true
00202 && (idx.getGivenValue() < 0 || idx.getGivenValue() > ((int) getNumRows( ) - 1)) )
00203 {
00204 LOG( LVL_ERROR )
00205 << "getRow: invalid row arguments, row index "
00206 << idx.getGivenValue() << " of variable " << getFullName()
00207 << " does not lie in the admissible range " << "0 - " << getNumRows( ) - 1 << endl;
00208 }
00209
00210 tmp->setSubmatrixOffsets(idx, 0, getNumRows(), getNumCols( ), 1, getNumCols( ));
00211
00212 return tmp;
00213 }
00214
00215
00216 ExportVariable ExportVariableInternal::getCol( const ExportIndex& idx
00217 ) const
00218 {
00219 ASSERT( doAccessTransposed == false );
00220
00221 ExportVariable tmp(name, data, type, dataStruct, callItByValue, prefix);
00222
00223 if (idx.isGiven() == true
00224 && (idx.getGivenValue() < 0 || idx.getGivenValue() > (int) getNumCols( ) - 1) )
00225 {
00226 LOG( LVL_ERROR )
00227 << "getCol: invalid column arguments, column index "
00228 << idx.getGivenValue() << " of variable " << getFullName()
00229 << " does not lie in the admissible range " << "0 - " << getNumCols( ) - 1 << endl;
00230 }
00231
00232 tmp->setSubmatrixOffsets(0, idx, getNumRows(), getNumCols( ), getNumRows(), 1);
00233
00234 return tmp;
00235 }
00236
00237
00238 ExportVariable ExportVariableInternal::getRows( const ExportIndex& idx1,
00239 const ExportIndex& idx2
00240 ) const
00241 {
00242 if (doAccessTransposed == true) ASSERT(data->getNumCols() == 1 || data->getNumRows() == 1);
00243
00244 ExportVariable tmp(name, data, type, dataStruct, callItByValue, prefix);
00245
00246 ExportIndex size = idx2 - idx1;
00247
00248 if (size.isGiven() == true && size.getGivenValue() == 0)
00249 return tmp;
00250
00251 if (idx1.isGiven() == true && idx2.isGiven() == true
00252 && ( idx1.getGivenValue() < 0 ||
00253 idx1.getGivenValue() > idx2.getGivenValue() ||
00254 idx2.getGivenValue() > (int) getNumRows( ) ))
00255 {
00256 LOG( LVL_ERROR ) << getFullName() << ": getRows: invalid row arguments" << endl;
00257 }
00258 else if (size.isGiven() == false)
00259 {
00260 LOG( LVL_ERROR ) << getFullName() << ": getRows: Cannot determine size" << endl;
00261 }
00262 else
00263 tmp->setSubmatrixOffsets(idx1, 0, getNumRows(), getNumCols( ), size.getGivenValue(), getNumCols( ));
00264
00265 return tmp;
00266 }
00267
00268
00269 ExportVariable ExportVariableInternal::getCols( const ExportIndex& idx1,
00270 const ExportIndex& idx2
00271 ) const
00272 {
00273 if (doAccessTransposed == true) ASSERT(data->getNumCols() == 1 || data->getNumRows() == 1);
00274
00275 ExportVariable tmp(name, data, type, dataStruct, callItByValue, prefix);
00276
00277 ExportIndex size = idx2 - idx1;
00278
00279 if (size.isGiven() == true && size.getGivenValue() == 0)
00280 return tmp;
00281
00282 if (idx1.isGiven() == true && idx2.isGiven() == true
00283 && ( idx1.getGivenValue() < 0 ||
00284 idx1.getGivenValue() > idx2.getGivenValue() ||
00285 idx2.getGivenValue() > (int) getNumCols( ) ))
00286 {
00287 LOG( LVL_ERROR ) << getFullName() << ": getCols: invalid column arguments" << endl;
00288 }
00289 else if (size.isGiven() == false)
00290 {
00291 LOG( LVL_ERROR ) << getFullName() << ": getCols: Cannot determine size" << endl;
00292 }
00293 else
00294 tmp->setSubmatrixOffsets(0, idx1, getNumRows(), getNumCols( ), getNumRows( ), size.getGivenValue() );
00295
00296 return tmp;
00297 }
00298
00299
00300 ExportVariable ExportVariableInternal::getSubMatrix( const ExportIndex& _rowIdx1,
00301 const ExportIndex& _rowIdx2,
00302 const ExportIndex& _colIdx1,
00303 const ExportIndex& _colIdx2
00304 ) const
00305 {
00306 ASSERT(doAccessTransposed == false);
00307
00308 ExportVariable tmp;
00309
00310 ExportIndex sizeRow = _rowIdx2 - _rowIdx1;
00311 ExportIndex sizeCol = _colIdx2 - _colIdx1;
00312
00313 if (sizeRow.isGiven() == true && sizeRow.getGivenValue() == 0)
00314 return tmp;
00315
00316 if (sizeCol.isGiven() == true && sizeCol.getGivenValue() == 0)
00317 return tmp;
00318
00319 if (_rowIdx1.isGiven() == true && _rowIdx2.isGiven() == true
00320 && ( _rowIdx1.getGivenValue() < 0 ||
00321 _rowIdx1.getGivenValue() > _rowIdx2.getGivenValue() ||
00322 _rowIdx2.getGivenValue() > (int) getNumRows( ) ))
00323 {
00324 LOG( LVL_ERROR ) << getFullName() << ": getSubMatrix: invalid row arguments" << endl;
00325 return tmp;
00326 }
00327 else if (sizeRow.isGiven() == false)
00328 {
00329 LOG( LVL_ERROR ) << getFullName() << ": getSubMatrix: cannot determine row size" << endl;
00330 return tmp;
00331 }
00332
00333 if (_colIdx1.isGiven() == true && _colIdx2.isGiven() == true
00334 && ( _colIdx1.getGivenValue() < 0 ||
00335 _colIdx1.getGivenValue() > _colIdx2.getGivenValue() ||
00336 _colIdx2.getGivenValue() > (int) getNumCols( ) ))
00337 {
00338 LOG( LVL_ERROR ) << getFullName() << ": getSubMatrix: invalid column arguments" << endl;
00339 return tmp;
00340 }
00341 else if (sizeCol.isGiven() == false)
00342 {
00343 LOG( LVL_ERROR ) << getFullName() << ": getSubMatrix: cannot determine column size" << endl;
00344 return tmp;
00345 }
00346
00347 tmp.assignNode(new ExportVariableInternal(name, data, type, dataStruct, callItByValue, prefix));
00348 tmp->setSubmatrixOffsets(_rowIdx1, _colIdx1, getNumRows(), getNumCols( ), sizeRow.getGivenValue(), sizeCol.getGivenValue());
00349
00350 return tmp;
00351 }
00352
00353
00354 ExportVariable ExportVariableInternal::makeRowVector( ) const
00355 {
00356 ASSERT( ( nRows == 0 ) && ( nCols == 0 ) );
00357
00358 DMatrix foo( *data.get() );
00359 foo.makeVector().transposeInPlace();
00360
00361 ExportVariable tmp(name, foo, type, dataStruct, callItByValue, prefix);
00362
00363 return tmp;
00364 }
00365
00366
00367 ExportVariable ExportVariableInternal::makeColVector( ) const
00368 {
00369 ASSERT( ( nRows == 0 ) && ( nCols == 0 ) );
00370
00371 DMatrix foo( *data.get() );
00372 foo.makeVector();
00373
00374 ExportVariable tmp(name, foo, type, dataStruct, callItByValue, prefix);
00375
00376 return tmp;
00377 }
00378
00379
00380 bool ExportVariableInternal::isVector( ) const
00381 {
00382 if (getNumRows( ) == 1 || getNumCols( ) == 1)
00383 return true;
00384
00385 return false;
00386 }
00387
00388
00389 const DMatrix& ExportVariableInternal::getGivenMatrix( ) const
00390 {
00391 if ( isGiven() == true )
00392 return *data.get();
00393
00394 return emptyConstMatrix;
00395 }
00396
00397
00398 returnValue ExportVariableInternal::print( ) const
00399 {
00400 return data->print( );
00401 }
00402
00403
00404
00405
00406
00407
00408
00409 uint ExportVariableInternal::getColDim( ) const
00410 {
00411 if (doAccessTransposed == true)
00412 return rowDim;
00413
00414 return colDim;
00415 }
00416
00417
00418 ExportIndex ExportVariableInternal::getTotalIdx( const ExportIndex& _rowIdx,
00419 const ExportIndex& _colIdx
00420 ) const
00421 {
00422 ExportIndex tmp;
00423
00424 if ( doAccessTransposed == false )
00425 tmp = _rowIdx * getColDim() + _colIdx;
00426 else
00427 tmp = _colIdx * getColDim() + _rowIdx;
00428
00429 return tmp;
00430 }
00431
00432
00433 returnValue ExportVariableInternal::setSubmatrixOffsets( const ExportIndex& _rowOffset,
00434 const ExportIndex& _colOffset,
00435 unsigned _rowDim,
00436 unsigned _colDim,
00437 unsigned _nRows,
00438 unsigned _nCols
00439 )
00440 {
00441 if ( ( _rowOffset.isGiven() == true ) && ( _rowOffset.getGivenValue() < 0 ) )
00442 return ACADOERROR( RET_INVALID_ARGUMENTS );
00443
00444 if ( ( _colOffset.isGiven() == true ) && ( _colOffset.getGivenValue() < 0 ) )
00445 return ACADOERROR( RET_INVALID_ARGUMENTS );
00446
00447 if ( ( _colOffset.isGiven() == true ) && ( _colOffset.getGivenValue() > (int)_colDim ) )
00448 return ACADOERROR( RET_INVALID_ARGUMENTS );
00449
00450 rowOffset = _rowOffset;
00451 colOffset = _colOffset;
00452 rowDim = _rowDim;
00453 colDim = _colDim;
00454
00455 nRows = _nRows;
00456 nCols = _nCols;
00457
00458 return SUCCESSFUL_RETURN;
00459 }
00460
00461
00462 bool ExportVariableInternal::hasValue( const ExportIndex& rowIdx,
00463 const ExportIndex& colIdx,
00464 double _value
00465 ) const
00466 {
00467 if ((getType() == STATIC_CONST_INT || getType() == STATIC_CONST_REAL) &&
00468 acadoIsEqual(_value, undefinedEntry) == true)
00469 return true;
00470
00471 ExportIndex ind = getTotalIdx(rowIdx + rowOffset, colIdx + colOffset);
00472
00473 if (ind.isGiven() == true)
00474 return acadoIsEqual(data->operator()(ind.getGivenValue()), _value);
00475
00476 return false;
00477 }
00478
00479 bool ExportVariableInternal::isSubMatrix() const
00480 {
00481 if (nRows == 0 && nCols == 0)
00482 return false;
00483
00484 return true;
00485 }
00486
00487 bool ExportVariableInternal::isDiagonal() const
00488 {
00489 if (isSubMatrix() == true)
00490 {
00491 LOG( LVL_DEBUG ) << "Digonal check works for non-sub-matrices only ATM" << endl;
00492 return false;
00493 }
00494
00495 DMatrix foo( *data );
00496 Eigen::DiagonalMatrix<double, Eigen::Dynamic, Eigen::Dynamic> bar( foo.diagonal() );
00497
00498 return (foo == bar);
00499 }
00500
00501
00502 CLOSE_NAMESPACE_ACADO
00503
00504