block_matrix.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
33 #include <acado/matrix_vector/matrix_vector.hpp>
34 
35 using namespace std;
36 
37 BEGIN_NAMESPACE_ACADO
38 
39 //
40 // PUBLIC MEMBER FUNCTIONS:
41 //
42 
43 BlockMatrix::BlockMatrix( )
44 {
45  nRows = nCols = 0;
46 }
47 
48 BlockMatrix::BlockMatrix( uint _nRows, uint _nCols )
49 {
50  init(_nRows, _nCols);
51 }
52 
53 
54 BlockMatrix::BlockMatrix( const DMatrix& value
55  )
56 {
57  init(1, 1);
58 
59  setDense(0, 0, value);
60 }
61 
62 BlockMatrix::~BlockMatrix( )
63 {}
64 
65 returnValue BlockMatrix::init( uint _nRows, uint _nCols )
66 {
67  nRows = _nRows;
68  nCols = _nCols;
69 
70  vector< DMatrix > foo(nCols, DMatrix());
71  for (unsigned row = 0; row < nRows; ++row)
72  elements.push_back( foo );
73 
74  vector< SubBlockMatrixType > bar(nCols, SBMT_ZERO);
75  for (unsigned row = 0; row < nRows; ++row)
76  types.push_back( bar );
77 
78  return SUCCESSFUL_RETURN;
79 }
80 
81 
82 BlockMatrix BlockMatrix::operator+( const BlockMatrix& arg ) const{
83 
84  ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
85 
86  uint i,j;
87 
88  BlockMatrix tmp( arg );
89 
90  for( i=0; i < getNumRows(); i++ ){
91  for( j=0; j < getNumCols(); j++ ){
92 
93  if( tmp.types[i][j] == SBMT_ZERO ){
94 
95  tmp.types [i][j] = types [i][j];
96  tmp.elements [i][j] = elements [i][j];
97  }
98  else{
99  if( types[i][j] != SBMT_ZERO ){
100 
101  tmp.types [i][j] = SBMT_DENSE ;
102  tmp.elements [i][j] += elements [i][j];
103  }
104  }
105  }
106  }
107 
108  return tmp;
109 }
110 
111 
112 BlockMatrix& BlockMatrix::operator+=( const BlockMatrix& arg ){
113 
114  ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
115 
116  uint i, j;
117 
118  for( i = 0; i < getNumRows(); i++ ){
119  for( j = 0; j < getNumCols(); j++ ){
120 
121  if( types[i][j] == SBMT_ZERO ){
122 
123  types [i][j] = arg.types [i][j];
124  elements [i][j] = arg.elements [i][j];
125  }
126  else{
127  if( arg.types[i][j] != SBMT_ZERO ){
128 
129  types [i][j] = SBMT_DENSE ;
130  elements [i][j] += arg.elements [i][j];
131  }
132  }
133  }
134  }
135  return *this;
136 }
137 
138 
139 BlockMatrix BlockMatrix::operator-( const BlockMatrix& arg ) const{
140 
141  ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
142 
143  uint i,j;
144 
145  BlockMatrix tmp( getNumRows(), getNumCols() );
146 
147  for( i=0; i < getNumRows(); i++ ){
148  for( j=0; j < getNumCols(); j++ ){
149 
150  if( arg.types[i][j] == SBMT_ZERO ){
151 
152  tmp.types [i][j] = types [i][j];
153  tmp.elements [i][j] = elements [i][j];
154  }
155  else{
156  if( types[i][j] != SBMT_ZERO ){
157 
158  tmp.types [i][j] = SBMT_DENSE;
159  tmp.elements [i][j] = elements [i][j] - arg.elements[i][j];
160  }
161  else{
162 
163  DMatrix nn(arg.elements[i][j].getNumRows(),arg.elements[i][j].getNumCols());
164  nn.setZero();
165 
166  tmp.types [i][j] = SBMT_DENSE;
167  tmp.elements [i][j] = nn - arg.elements[i][j];
168  }
169  }
170  }
171  }
172 
173  return tmp;
174 }
175 
176 BlockMatrix BlockMatrix::operator*=( double scalar ){
177 
178  uint i,j;
179 
180  for( i=0; i < getNumRows(); i++ ){
181  for( j=0; j < getNumCols(); j++ ){
182  if( types[i][j] != SBMT_ZERO ){
183  types [i][j] = SBMT_DENSE;
184  elements[i][j] *= scalar ;
185  }
186  }
187  }
188  return *this;
189 }
190 
191 
192 BlockMatrix BlockMatrix::operator*( const BlockMatrix& arg ) const{
193 
194  ASSERT( getNumCols( ) == arg.getNumRows( ) );
195 
196  uint i,j,k;
197 
198  uint newNumRows = getNumRows( );
199  uint newNumCols = arg.getNumCols( );
200  BlockMatrix result( newNumRows,newNumCols );
201 
202  for( i=0; i<newNumRows; ++i ){
203  for( k=0; k<getNumCols( ); ++k ){
204 
205  switch( types[i][k] ){
206 
207  case SBMT_DENSE:
208 
209  for( j=0; j<newNumCols; ++j ){
210 
211  if( arg.types[k][j] == SBMT_DENSE ){
212 
213  if( result.types[i][j] != SBMT_ZERO )
214  result.elements[i][j] += elements[i][k] * arg.elements[k][j];
215  else result.elements[i][j] = elements[i][k] * arg.elements[k][j];
216  }
217 
218  if( arg.types[k][j] == SBMT_ONE ){
219 
220  if( result.types[i][j] != SBMT_ZERO )
221  result.elements[i][j] += elements[i][k];
222  else result.elements[i][j] = elements[i][k];
223  }
224 
225  if( arg.types[k][j] != SBMT_ZERO )
226  result.types[i][j] = SBMT_DENSE;
227  }
228  break;
229 
230 
231  case SBMT_ONE:
232 
233  for( j=0; j<newNumCols; ++j ){
234 
235  if( arg.types[k][j] == SBMT_DENSE ){
236 
237  if( result.types[i][j] != SBMT_ZERO )
238  result.elements[i][j] += arg.elements[k][j];
239  else result.elements[i][j] = arg.elements[k][j];
240 
241  result.types[i][j] = SBMT_DENSE;
242  }
243 
244  if( arg.types[k][j] == SBMT_ONE ){
245 
246  if( result.types[i][j] == SBMT_ZERO ){
247  result.elements[i][j] = elements[i][k];
248  result.types [i][j] = SBMT_ONE ;
249  }
250  else{
251  result.elements[i][j] += elements[i][k];
252  result.types [i][j] = SBMT_DENSE ;
253  }
254  }
255  }
256  break;
257 
258  case SBMT_ZERO:
259 
260  break;
261 
262  default:
263  break;
264  }
265  }
266  }
267 
268  return result;
269 }
270 
271 
272 BlockMatrix BlockMatrix::operator^( const BlockMatrix& arg ) const{
273 
274  ASSERT( getNumRows( ) == arg.getNumRows( ) );
275 
276  uint i,j,k;
277 
278  uint newNumRows = getNumCols( );
279  uint newNumCols = arg.getNumCols( );
280  BlockMatrix result( newNumRows,newNumCols );
281 
282  for( i=0; i<newNumRows; ++i ){
283  for( k=0; k<getNumRows( ); ++k ){
284 
285  switch( types[k][i] ){
286 
287  case SBMT_DENSE:
288 
289  for( j=0; j<newNumCols; ++j ){
290 
291  if( arg.types[k][j] == SBMT_DENSE ){
292  if( result.types[i][j] != SBMT_ZERO )
293  result.elements[i][j] += elements[k][i].transpose() * arg.elements[k][j];
294  else result.elements[i][j] = elements[k][i].transpose() * arg.elements[k][j];
295  }
296 
297  if( arg.types[k][j] == SBMT_ONE ){
298  if( result.types[i][j] != SBMT_ZERO )
299  result.elements[i][j] += elements[k][i].transpose();
300  else result.elements[i][j] = elements[k][i].transpose();
301  }
302 
303  if( arg.types[k][j] != SBMT_ZERO )
304  result.types[i][j] = SBMT_DENSE;
305  }
306  break;
307 
308 
309  case SBMT_ONE:
310 
311  for( j=0; j<newNumCols; ++j ){
312 
313  if( arg.types[k][j] == SBMT_DENSE ){
314  if( result.types[i][j] != SBMT_ZERO )
315  result.elements[i][j] += arg.elements[k][j];
316  else result.elements[i][j] = arg.elements[k][j];
317  result.types[i][j] = SBMT_DENSE;
318  }
319 
320  if( arg.types[k][j] == SBMT_ONE ){
321 
322  if( result.types[i][j] == SBMT_ZERO ){
323  result.elements[i][j] = elements[k][i];
324  result.types [i][j] = SBMT_ONE ;
325  }
326  else{
327  result.elements[i][j] += elements[k][i];
328  result.types [i][j] = SBMT_DENSE ;
329  }
330  }
331  }
332  break;
333 
334  case SBMT_ZERO:
335  break;
336 
337  default:
338  break;
339  }
340  }
341  }
342  return result;
343 }
344 
345 
346 BlockMatrix BlockMatrix::transpose() const{
347 
348  BlockMatrix result( getNumCols(), getNumRows() );
349 
350  uint i,j;
351 
352  for( i = 0; i < getNumRows(); i++ ){
353  for( j = 0; j < getNumCols(); j++ ){
354  result.elements[j][i] = elements[i][j].transpose();
355  result.types [j][i] = types [i][j] ;
356  }
357  }
358 
359  return result;
360 }
361 
362 
363 BlockMatrix BlockMatrix::getAbsolute() const{
364 
365  uint run1, run2;
366  BlockMatrix result( nRows, nCols );
367 
368  for( run1 = 0; run1 < nRows; run1++ ){
369  for( run2 = 0; run2 < nCols; run2++ ){
370 
371  if( types[run1][run2] == SBMT_ONE )
372  result.setIdentity( run1, run2, elements[run1][run2].getNumRows() );
373 
374  if( types[run1][run2] == SBMT_DENSE )
375  result.setDense( run1, run2, elements[run1][run2].absolute() );
376  }
377  }
378 
379  return result;
380 }
381 
382 
383 BlockMatrix BlockMatrix::getPositive() const{
384 
385  uint run1, run2;
386  BlockMatrix result( nRows, nCols );
387 
388  for( run1 = 0; run1 < nRows; run1++ ){
389  for( run2 = 0; run2 < nCols; run2++ ){
390 
391  if( types[run1][run2] == SBMT_ONE )
392  result.setIdentity( run1, run2, elements[run1][run2].getNumRows() );
393 
394  if( types[run1][run2] == SBMT_DENSE )
395  result.setDense( run1, run2, elements[run1][run2].positive() );
396  }
397  }
398 
399  return result;
400 }
401 
402 
403 BlockMatrix BlockMatrix::getNegative() const{
404 
405  uint run1, run2;
406  BlockMatrix result( nRows, nCols );
407 
408  for( run1 = 0; run1 < nRows; run1++ ){
409  for( run2 = 0; run2 < nCols; run2++ ){
410 
411  if( types[run1][run2] == SBMT_DENSE )
412  result.setDense( run1, run2, elements[run1][run2].negative() );
413  }
414  }
415 
416  return result;
417 }
418 
419 
420 returnValue BlockMatrix::print( std::ostream& stream) const
421 {
422  stream << "Printing Block DMatrix:" << endl << endl;
423 
424  for (unsigned i = 0; i < getNumRows(); ++i)
425  {
426  stream << "Row " << i << endl;
427  for (unsigned j = 0; j < getNumCols(); ++j)
428  {
429  if(nRows * nCols)
430  {
431  if( types[i][j] == SBMT_DENSE ) stream << elements[i][j] << endl;
432  if( types[i][j] == SBMT_ONE ) stream << "ONE " << endl;
433  if( types[i][j] == SBMT_ZERO ) stream << "ZERO " << endl;
434  }
435  else
436  stream << "ZERO " << endl;
437  stream << endl;
438  }
439  stream << endl << endl;
440  }
441 
442  return SUCCESSFUL_RETURN;
443 }
444 
445 
446 returnValue BlockMatrix::setDense( uint rowIdx, uint colIdx, const DMatrix& value ){
447 
448  ASSERT( rowIdx < getNumRows( ) );
449  ASSERT( colIdx < getNumCols( ) );
450 
451  elements[rowIdx][colIdx] = value ;
452  types [rowIdx][colIdx] = SBMT_DENSE;
453 
454  return SUCCESSFUL_RETURN;
455 }
456 
457 
458 returnValue BlockMatrix::addDense( uint rowIdx, uint colIdx, const DMatrix& value ){
459 
460  ASSERT( rowIdx < getNumRows( ) );
461  ASSERT( colIdx < getNumCols( ) );
462 
463  if( types[rowIdx][colIdx] == SBMT_DENSE || types[rowIdx][colIdx] == SBMT_ONE ){
464  types[rowIdx][colIdx] = SBMT_DENSE;
465  elements[rowIdx][colIdx] += value;
466  return SUCCESSFUL_RETURN;
467  }
468 
469  return setDense( rowIdx, colIdx, value );
470 }
471 
472 
473 returnValue BlockMatrix::getSubBlock( uint rowIdx, uint colIdx,
474  DMatrix &value, uint nR, uint nC ) const{
475 
476 
477  ASSERT( rowIdx < getNumRows( ) );
478  ASSERT( colIdx < getNumCols( ) );
479 
480 
481  if( types[rowIdx][colIdx] != SBMT_ZERO ){
482 
483  ASSERT( nR == elements[rowIdx][colIdx].getNumRows( ) );
484  ASSERT( nC == elements[rowIdx][colIdx].getNumCols( ) );
485 
486  value = elements[rowIdx][colIdx];
487  }
488  else{
489  value.resize(nR, nC);
490  value.setZero();
491  }
492 
493  return SUCCESSFUL_RETURN;
494 }
495 
496 CLOSE_NAMESPACE_ACADO
497 
498 /*
499  * end of file
500  */
BlockMatrix
Implements a very rudimentary block sparse matrix class.
Definition: block_matrix.hpp:52
BlockMatrix::~BlockMatrix
virtual ~BlockMatrix()
Definition: block_matrix.cpp:62
BlockMatrix::print
returnValue print(std::ostream &stream=std::cout) const
Definition: block_matrix.cpp:420
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
BlockMatrix::transpose
BlockMatrix transpose() const
Definition: block_matrix.cpp:346
BlockMatrix::setDense
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:446
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
BlockMatrix::operator^
BlockMatrix operator^(const BlockMatrix &arg) const
Definition: block_matrix.cpp:272
BlockMatrix::getAbsolute
BlockMatrix getAbsolute() const
Definition: block_matrix.cpp:363
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
BlockMatrix::operator+
BlockMatrix operator+(const BlockMatrix &arg) const
Definition: block_matrix.cpp:82
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
BlockMatrix::getNegative
BlockMatrix getNegative() const
Definition: block_matrix.cpp:403
DMatrix
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
BlockMatrix::operator-
BlockMatrix operator-(const BlockMatrix &arg) const
Definition: block_matrix.cpp:139
BlockMatrix::getSubBlock
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
BlockMatrix::types
std::vector< std::vector< SubBlockMatrixType > > types
Definition: block_matrix.hpp:215
Eigen::PlainObjectBase::resize
EIGEN_STRONG_INLINE void resize(Index nbRows, Index nbCols)
Definition: PlainObjectBase.h:232
BlockMatrix::init
returnValue init(uint _nRows, uint _nCols)
Definition: block_matrix.cpp:65
BlockMatrix::operator+=
BlockMatrix & operator+=(const BlockMatrix &arg)
Definition: block_matrix.cpp:112
Eigen::PlainObjectBase::setZero
Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:515
matrix_vector.hpp
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
SBMT_ZERO
Definition: acado_types.hpp:128
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
SBMT_ONE
Definition: acado_types.hpp:129
BlockMatrix::getPositive
BlockMatrix getPositive() const
Definition: block_matrix.cpp:383
BlockMatrix::operator*
BlockMatrix operator*(const BlockMatrix &arg) const
Definition: block_matrix.cpp:192
BlockMatrix::getNumCols
uint getNumCols() const
BlockMatrix::operator*=
BlockMatrix operator*=(double scalar)
Definition: block_matrix.cpp:176
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
SBMT_DENSE
Definition: acado_types.hpp:130
BlockMatrix::setIdentity
returnValue setIdentity(uint rowIdx, uint colIdx, uint dim)
init
void init(int nV, int nC, SymmetricMatrix *H, real_t *g, Matrix *A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int nWSR, const real_t *const x0, Options *options, int nOutputs, mxArray *plhs[])
Definition: qpOASES-3.0beta/interfaces/matlab/qpOASES_sequence.cpp:111
BlockMatrix::addDense
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:458
BlockMatrix::elements
std::vector< std::vector< DMatrix > > elements
Definition: block_matrix.hpp:214
BlockMatrix::getNumRows
uint getNumRows() const

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:29