1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include <boost/numeric/ublas/matrix.hpp>
20 #include <boost/numeric/ublas/matrix_proxy.hpp>
21
22 #include "SiconosVector.hpp"
23 #include "SimpleMatrix.hpp"
24 #include "BlockMatrixIterators.hpp"
25 #include "BlockMatrix.hpp"
26
27 #include "SiconosAlgebra.hpp"
28 #include "SiconosException.hpp"
29
30 using namespace Siconos;
31
32 //=============================
33 // Elements access (get or set)
34 //=============================
35
getValue(unsigned int row,unsigned int col) const36 double SimpleMatrix::getValue(unsigned int row, unsigned int col) const
37 {
38 if(row >= size(0) || col >= size(1))
39 THROW_EXCEPTION("Index out of range");
40
41 if(_num == Siconos::DENSE)
42 return (*mat.Dense)(row, col);
43 else if(_num == Siconos::TRIANGULAR)
44 return (*mat.Triang)(row, col);
45 else if(_num == Siconos::SYMMETRIC)
46 return (*mat.Sym)(row, col);
47 else if(_num == Siconos::SPARSE)
48 {
49 double * d = (*mat.Sparse).find_element(row, col);
50 if(d)
51 return *d;
52 else
53 return 0.0;
54 }
55 else if(_num == Siconos::SPARSE_COORDINATE)
56 {
57 double * d = (*mat.SparseCoordinate).find_element(row, col);
58 if(d)
59 return *d;
60 else
61 return 0.0;
62 }
63 else if(_num == Siconos::BANDED)
64 return (*mat.Banded)(row, col);
65 else if(_num == Siconos::ZERO)
66 return 0;
67 else //if (_num == Siconos::IDENTITY)
68 return(row == col);
69 }
70
setValue(unsigned int row,unsigned int col,double value)71 void SimpleMatrix::setValue(unsigned int row, unsigned int col, double value)
72 {
73 if(row >= size(0) || col >= size(1))
74 THROW_EXCEPTION("Index out of range");
75
76 if(_num == Siconos::DENSE)
77 (*mat.Dense)(row, col) = value;
78 else if(_num == Siconos::TRIANGULAR)
79 (*mat.Triang)(row, col) = value;
80 else if(_num == Siconos::SYMMETRIC)
81 (*mat.Sym)(row, col) = value ;
82 else if(_num == Siconos::SPARSE)
83 {
84 double * d = (*mat.Sparse).find_element(row, col);
85 if(d)
86 {
87 *d = value;
88 }
89 else
90 {
91 (*mat.Sparse).insert_element(row, col, value);
92 }
93 }
94 else if(_num == Siconos::SPARSE_COORDINATE)
95 {
96 // double * d = (*mat.Sparse).find_element(row, col);
97 // if (d)
98 // {
99 // *d = value;
100 // }
101 // else
102 // {
103 (*mat.SparseCoordinate).insert_element(row, col, value);
104 // }
105 }
106
107 else if(_num == Siconos::BANDED)
108 (*mat.Banded)(row, col) = value;
109 else if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
110 THROW_EXCEPTION("orbidden for Identity or Zero type matrices.");
111 resetFactorizationFlags();
112
113 }
114
115 //============================================
116 // Access (get or set) to blocks of elements
117 //============================================
118
setBlock(unsigned int row_min,unsigned int col_min,const SiconosMatrix & m)119 void SimpleMatrix::setBlock(unsigned int row_min, unsigned int col_min, const SiconosMatrix& m)
120 {
121 // Set current matrix elements, starting from row row_min and column col_min, with the values of the matrix m.
122 // m may be a BlockMatrix.
123
124 if(&m == this)
125 THROW_EXCEPTION("m = this.");
126
127 if(row_min >= size(0))
128 THROW_EXCEPTION("row is out of range");
129
130 if(col_min >= size(1))
131 THROW_EXCEPTION("col is out of range");
132
133 unsigned int row_max, col_max;
134 row_max = m.size(0) + row_min;
135 col_max = m.size(1) + col_min;
136
137 if(row_max > size(0))
138 THROW_EXCEPTION("row is out of range.");
139
140 if(col_max > size(1))
141 THROW_EXCEPTION("m.col + col is out of range.");
142
143 Siconos::UBLAS_TYPE numM = m.num();
144
145 if(numM == Siconos::BLOCK) // if m is a block matrix ...
146 {
147 const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
148 BlocksMat::const_iterator1 it;
149 BlocksMat::const_iterator2 it2;
150 unsigned int posRow = row_min;
151 unsigned int posCol = col_min;
152
153 for(it = mB._mat->begin1(); it != mB._mat->end1(); ++it)
154 {
155 for(it2 = it.begin(); it2 != it.end(); ++it2)
156 {
157 setBlock(posRow, posCol, **it2);
158 posCol += (*it2)->size(1);
159 }
160 posRow += (*it)->size(0);
161 posCol = col_min;
162 }
163 }
164 else // if m is a SimpleMatrix
165 {
166 if(numM != _num)
167 THROW_EXCEPTION("Inconsistent matrix types.");
168
169 if(_num == Siconos::DENSE)
170 noalias(ublas::subrange(*mat.Dense, row_min, row_max, col_min, col_max)) = *(m.dense());
171 else if(_num == Siconos::TRIANGULAR)
172 noalias(ublas::subrange(*mat.Triang, row_min, row_max, col_min, col_max)) = *(m.triang());
173 else if(_num == Siconos::SYMMETRIC)
174 noalias(ublas::subrange(*mat.Sym, row_min, row_max, col_min, col_max)) = *(m.sym());
175 else if(_num == Siconos::SPARSE)
176 noalias(ublas::subrange(*mat.Sparse, row_min, row_max, col_min, col_max)) = *(m.sparse());
177 else if(_num == Siconos::BANDED)
178 noalias(ublas::subrange(*mat.Banded, row_min, row_max, col_min, col_max)) = *(m.banded());
179 else // if(_num == Siconos::ZERO) or _num == Siconos::IDENTITY nothing to do
180 {}
181 resetFactorizationFlags();
182 }
183 }
184
getRow(unsigned int r,SiconosVector & vOut) const185 void SimpleMatrix::getRow(unsigned int r, SiconosVector &vOut) const
186 {
187 // Get row number r of current matrix and copy it into vOut.
188 if(r >= size(0))
189 THROW_EXCEPTION("row is out of range");
190
191 if(vOut.size() != size(1))
192 THROW_EXCEPTION("inconsistent sizes between this and v.");
193
194 if(_num == Siconos::IDENTITY) // identity matrix
195 {
196 vOut.zero();
197 vOut(r) = 1.0;
198 }
199 else if(_num == Siconos::ZERO) // Zero matrix
200 vOut.zero();
201 else
202 {
203 Siconos::UBLAS_TYPE numV = vOut.num();
204 if(numV == Siconos::DENSE)
205 {
206 if(_num == Siconos::DENSE)
207 {
208 noalias(*(vOut.dense())) = ublas::row(*mat.Dense, r);
209 }
210 else if(_num == Siconos::TRIANGULAR)
211 {
212 noalias(*(vOut.dense())) = ublas::row(*mat.Triang, r);
213 }
214 else if(_num == Siconos::SYMMETRIC)
215 {
216 noalias(*(vOut.dense())) = ublas::row(*mat.Sym, r);
217 }
218 else if(_num == Siconos::SPARSE)
219 {
220 noalias(*(vOut.dense())) = ublas::row(*mat.Sparse, r);
221 }
222 else //if(_num == Siconos::BANDED){
223 noalias(*(vOut.dense())) = ublas::row(*mat.Banded, r);
224 }
225 else // if numV == Siconos::SPARSE
226 {
227 if(_num == Siconos::SPARSE)
228 {
229 noalias(*(vOut.sparse())) = ublas::row(*mat.Sparse, r);
230 }
231 else
232 THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
233 }
234 }
235 }
236
setRow(unsigned int r,const SiconosVector & vIn)237 void SimpleMatrix::setRow(unsigned int r, const SiconosVector& vIn)
238 {
239 // Set row number r of current matrix with vIn.
240 Siconos::UBLAS_TYPE numV = vIn.num();
241 if(r >= size(0))
242 THROW_EXCEPTION("row is out of range");
243
244 if(vIn.size() != size(1))
245 THROW_EXCEPTION("inconsistent sizes between this and v.");
246
247 if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
248 THROW_EXCEPTION("current matrix is read-only (zero or identity).");
249
250 {
251 if(_num == Siconos::DENSE)
252 {
253 if(numV == Siconos::DENSE)
254 {
255 noalias(ublas::row(*mat.Dense, r)) = *vIn.dense();
256 }
257 else if(numV == Siconos::SPARSE)
258 {
259 noalias(ublas::row(*mat.Dense, r)) = *vIn.sparse();
260 }
261 }
262 else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
263 noalias(ublas::row(*mat.Sparse, r)) = *vIn.sparse();
264 else
265 THROW_EXCEPTION("inconsistent types between current matrix and v.");
266 }
267
268 resetFactorizationFlags();
269 }
270
getCol(unsigned int r,SiconosVector & vOut) const271 void SimpleMatrix::getCol(unsigned int r, SiconosVector &vOut)const
272 {
273 // Get column number r of current matrix and copy it into vOut.
274 if(r >= size(1))
275 THROW_EXCEPTION("col is out of range");
276
277 if(vOut.size() != size(0))
278 THROW_EXCEPTION("inconsistent sizes between this and v.");
279
280 if(_num == Siconos::IDENTITY) // identity matrix
281 {
282 vOut.zero();
283 vOut(r) = 1.0;
284 }
285 else if(_num == Siconos::ZERO) // Zero matrix
286 vOut.zero();
287 else
288 {
289 Siconos::UBLAS_TYPE numV = vOut.num();
290
291 if(numV == Siconos::DENSE)
292 {
293
294 if(_num == Siconos::DENSE)
295 {
296 noalias(*(vOut.dense())) = ublas::column(*mat.Dense, r);
297 }
298 else if(_num == Siconos::TRIANGULAR)
299 {
300 noalias(*(vOut.dense())) = ublas::column(*mat.Triang, r);
301 }
302 else if(_num == Siconos::SYMMETRIC)
303 {
304 noalias(*(vOut.dense())) = ublas::column(*mat.Sym, r);
305 }
306 else if(_num == Siconos::SPARSE)
307 {
308 noalias(*(vOut.dense())) = ublas::column(*mat.Sparse, r);
309 }
310 else //if(_num == Siconos:BANDED){
311 noalias(*(vOut.dense())) = ublas::column(*mat.Banded, r);
312 }
313 else // if _numV == Siconos::SPARSE
314 {
315 if(_num == Siconos::SPARSE)
316 {
317 noalias(*(vOut.sparse())) = ublas::column(*mat.Sparse, r);
318 }
319 else
320 THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
321 }
322 }
323 }
324
setCol(unsigned int r,const SiconosVector & vIn)325 void SimpleMatrix::setCol(unsigned int r, const SiconosVector &vIn)
326 {
327 // Set column number r of current matrix with vIn.
328 Siconos::UBLAS_TYPE numV = vIn.num();
329 if(r >= size(1))
330 THROW_EXCEPTION("col is out of range");
331
332 if(vIn.size() != size(0))
333 THROW_EXCEPTION("inconsistent sizes between this and v.");
334
335 if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
336 THROW_EXCEPTION("current matrix is read-only (zero or identity).");
337
338 {
339 if(_num == Siconos::DENSE)
340 {
341 if(numV == Siconos::DENSE)
342 {
343 noalias(ublas::column(*mat.Dense, r)) = *vIn.dense();
344 }
345 else if(numV == Siconos::SPARSE)
346 {
347 noalias(ublas::column(*mat.Dense, r)) = *vIn.sparse();
348 }
349 }
350 else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
351 noalias(ublas::column(*mat.Sparse, r)) = *vIn.sparse();
352 else
353 THROW_EXCEPTION("nconsistent types between current matrix and v.");
354 }
355
356 resetFactorizationFlags();
357 }
358
getSubRow(unsigned int r,unsigned int pos,SP::SiconosVector vOut) const359 void SimpleMatrix::getSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
360 {
361 // Get row number r of current matrix, starting from element at position pos, and copy it into vOut.
362 if(r >= size(0))
363 THROW_EXCEPTION("row is out of range");
364
365 if(vOut->size() > size(1) - pos)
366 THROW_EXCEPTION("inconsistent sizes between this and v.");
367
368 if(_num == Siconos::IDENTITY) // identity matrix
369 {
370 vOut->zero();
371 if(r >= pos)
372 (*vOut)(r - pos) = 1.0;
373 }
374 else if(_num == Siconos::ZERO) // Zero matrix
375 vOut->zero();
376 else
377 {
378 Siconos::UBLAS_TYPE numV = vOut->num();
379 unsigned int nbEl = vOut->size();
380
381 if(numV == Siconos::DENSE)
382 {
383 if(_num == Siconos::DENSE)
384 {
385 // noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
386 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
387 }
388 else if(_num == Siconos::TRIANGULAR)
389 {
390 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
391 }
392 else if(_num == Siconos::SYMMETRIC)
393 {
394 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
395 }
396 else if(_num == Siconos::SPARSE)
397 {
398 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
399 }
400 else //if(_num == Siconos::BANDED){
401 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
402 }
403 else // if numV == Siconos::SPARSE
404 {
405 if(_num == Siconos::SPARSE)
406 {
407 #ifdef BOOST_LIMITATION
408 THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
409 #else
410 noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl));
411 #endif
412 }
413 else
414 THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
415 }
416 }
417
418 }
419
setSubRow(unsigned int r,unsigned int pos,SP::SiconosVector vIn)420 void SimpleMatrix::setSubRow(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
421 {
422 // Set row number r, starting from element at position pos, of current matrix with vIn.
423 Siconos::UBLAS_TYPE numV = vIn->num();
424 if(r >= size(0))
425 THROW_EXCEPTION("row is out of range");
426
427 if(vIn->size() > size(1) - pos)
428 THROW_EXCEPTION("inconsistent sizes between this and v.");
429
430 if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
431 THROW_EXCEPTION("current matrix is read-only (zero or identity).");
432
433 {
434 unsigned int nbEl = vIn->size();
435 if(_num == Siconos::DENSE)
436 {
437 if(numV == Siconos::DENSE)
438 {
439 noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl))) = *vIn->dense();
440 }
441 else if(numV == Siconos::SPARSE)
442 {
443 ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
444 }
445 }
446 else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
447 #ifdef BOOST_LIMITATION
448 THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
449 #else
450 ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(r, 0, nbEl), ublas::slice(pos, 1, nbEl)) = *vIn->sparse();
451 #endif
452 else
453 THROW_EXCEPTION("inconsistent types between current matrix and v.");
454 resetFactorizationFlags();
455 }
456
457 }
458
getSubCol(unsigned int r,unsigned int pos,SP::SiconosVector vOut) const459 void SimpleMatrix::getSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vOut) const
460 {
461 // Get col _number r of current matrix, starting from element at position pos, and copy it into vOut.
462 if(r >= size(1))
463 THROW_EXCEPTION("col is out of range");
464
465 if(vOut->size() > size(0) - pos)
466 THROW_EXCEPTION("inconsistent sizes between this and v.");
467
468 if(_num == Siconos::IDENTITY) // identity matrix
469 {
470 vOut->zero();
471 if(r >= pos)
472 (*vOut)(r - pos) = 1.0;
473 }
474 else if(_num == Siconos::ZERO) // Zero matrix
475 vOut->zero();
476 else
477 {
478 Siconos::UBLAS_TYPE numV = vOut->num();
479 unsigned int nbEl = vOut->size();
480
481 if(numV == Siconos::DENSE)
482 {
483 if(_num == Siconos::DENSE)
484 {
485 // noalias(*(vOut->dense())) = ublas::row(ublas::subrange(*mat.Dense, r, r+1,pos, endPos),0);
486 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
487 }
488 else if(_num == Siconos::TRIANGULAR)
489 {
490 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<TriangMat >(*mat.Triang, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
491 }
492 else if(_num == Siconos::SYMMETRIC)
493 {
494 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SymMat >(*mat.Sym, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
495 }
496 else if(_num == Siconos::SPARSE)
497 {
498 #ifdef BOOST_LIMITATION
499 THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
500 #else
501 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
502 #endif
503 }
504 else //if(_num == Siconos::BANDED){
505 noalias(*(vOut->dense())) = ublas::matrix_vector_slice<BandedMat >(*mat.Banded, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
506 }
507 else // if numV == Siconos::SPARSE
508 {
509 if(_num == Siconos::SPARSE)
510 {
511 #ifdef BOOST_LIMITATION
512 THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
513 #else
514 noalias(*(vOut->sparse())) = ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl));
515 #endif
516 }
517 else
518 THROW_EXCEPTION("inconsistent types between this (not sparse) and v (sparse).");
519 }
520 }
521
522 }
523
setSubCol(unsigned int r,unsigned int pos,SP::SiconosVector vIn)524 void SimpleMatrix::setSubCol(unsigned int r, unsigned int pos, SP::SiconosVector vIn)
525 {
526 // Set column number r, starting from element at position pos, of current matrix with vIn.
527 Siconos::UBLAS_TYPE numV = vIn->num();
528 if(r >= size(1))
529 THROW_EXCEPTION("col is out of range");
530
531 if(vIn->size() > size(0) - pos)
532 THROW_EXCEPTION("inconsistent sizes between this and v.");
533
534 if(_num == Siconos::ZERO || _num == Siconos::IDENTITY)
535 THROW_EXCEPTION("current matrix is read-only (zero or identity).");
536
537 {
538 unsigned int nbEl = vIn->size();
539 if(_num == Siconos::DENSE)
540 {
541 if(numV == Siconos::DENSE)
542 {
543 noalias(ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl))) = *vIn->dense();
544 }
545 else if(numV == Siconos::SPARSE)
546 {
547 ublas::matrix_vector_slice<DenseMat >(*mat.Dense, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
548 }
549 }
550 else if(_num == Siconos::SPARSE && numV == Siconos::SPARSE)
551 #ifdef BOOST_LIMITATION
552 THROW_EXCEPTION("ublas::matrix_vector_slice<SparseMat> does not exist for your boost distribution and your architecture.");
553 #else
554 ublas::matrix_vector_slice<SparseMat >(*mat.Sparse, ublas::slice(pos, 1, nbEl), ublas::slice(r, 0, nbEl)) = *vIn->sparse();
555 #endif
556 else
557 THROW_EXCEPTION("inconsistent types between current matrix and v.");
558 resetFactorizationFlags();
559 }
560 }
561
562
563