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 "SimpleMatrix.hpp"
20 #include "BlockMatrixIterators.hpp"
21 #include "BlockMatrix.hpp"
22
23 #include "SiconosAlgebra.hpp"
24 #include "SimpleMatrixFriends.hpp" // for operators
25 #include "SiconosException.hpp"
26 using namespace Siconos;
27 //#define DEBUG_MESSAGES
28 #include "siconos_debug.h"
29
operator *(const SP::SimpleMatrix A,const SP::SimpleMatrix B)30 SP::SimpleMatrix operator * (const SP::SimpleMatrix A, const SP::SimpleMatrix B)
31 {
32 SP::SimpleMatrix aux(new SimpleMatrix((DenseMat)prod(*(*A).dense(), *(*B).dense())));
33 return aux;
34 }
35
operator *(const SiconosMatrix & A,double a)36 const SimpleMatrix operator * (const SiconosMatrix & A, double a)
37 {
38 // To compute B = a * A
39
40 Siconos::UBLAS_TYPE numA = A.num();
41
42 if(numA == ZERO) // if A = 0
43 {
44 //DenseMat p(zero_matrix(A.size(0),A.size(1)));
45 //return p;
46 return A;
47 }
48 else if(numA == IDENTITY)
49 {
50 return (DenseMat)(a**A.identity());
51 }
52 else if(numA == Siconos::BLOCK) // A block
53 {
54 SimpleMatrix tmp(A); // ... copy ...
55 tmp *= a;
56 return tmp;
57 }
58 else if(numA == DENSE) // dense)
59 return (DenseMat)(a** A.dense());
60 else if(numA == TRIANGULAR)
61 return (TriangMat)(a ** A.triang());
62 else if(numA == SYMMETRIC)
63 return (SymMat)(a ** A.sym());
64 else if(numA == SPARSE)
65 return (SparseMat)(a ** A.sparse());
66 else if(numA == BANDED)
67 return (BandedMat)(a ** A.banded());
68 else
69 {
70 THROW_EXCEPTION("invalid type of matrix");
71 }
72 }
73
operator *(double a,const SiconosMatrix & A)74 SimpleMatrix operator * (double a, const SiconosMatrix & A)
75 {
76 // To compute B = a * A
77
78 Siconos::UBLAS_TYPE numA = A.num();
79
80 if(numA == ZERO) // if A = 0
81 {
82 //DenseMat p(zero_matrix(A.size(0),A.size(1)));
83 //return p;
84 return A;
85 }
86 else if(numA == IDENTITY)
87 {
88 return (DenseMat)(a**A.identity());
89 }
90 else if(numA == 0) // A block
91 {
92 SimpleMatrix tmp(A); // ... copy ...
93 tmp *= a;
94 return tmp;
95 }
96 else if(numA == DENSE) // dense)
97 return (DenseMat)(a** A.dense());
98 else if(numA == TRIANGULAR)
99 return (TriangMat)(a ** A.triang());
100 else if(numA == SYMMETRIC)
101 return (SymMat)(a ** A.sym());
102 else if(numA == SPARSE)
103 return (SparseMat)(a ** A.sparse());
104 else if(numA == SPARSE_COORDINATE)
105 return (SparseCoordinateMat)(a ** A.sparseCoordinate());
106 else if(numA == BANDED)
107 return (BandedMat)(a ** A.banded());
108 else
109 {
110 THROW_EXCEPTION("invalid type of matrix");
111 }
112 }
113
operator /(const SiconosMatrix & A,double a)114 const SimpleMatrix operator / (const SiconosMatrix & A, double a)
115 {
116 // To compute B = A/a
117
118 if(a == 0.0)
119 THROW_EXCEPTION("division by zero.");
120
121 Siconos::UBLAS_TYPE numA = A.num();
122
123 if(numA == ZERO) // if A = 0
124 {
125 //DenseMat p(zero_matrix(A.size(0),A.size(1)));
126 //return p;
127 return A;
128 }
129 else if(numA == IDENTITY)
130 {
131 return (DenseMat)(*A.identity() / a);
132 }
133 else if(numA == 0) // A block
134 {
135 SimpleMatrix tmp(A); // ... copy ...
136 tmp /= a;
137 return tmp;
138 }
139 else if(numA == DENSE) // dense)
140 return (DenseMat)(*A.dense() / a);
141 else if(numA == TRIANGULAR)
142 return (TriangMat)(*A.triang() / a);
143 else if(numA == SYMMETRIC)
144 return (SymMat)(*A.sym() / a);
145 else if(numA == SPARSE)
146 return (SparseMat)(*A.sparse() / a);
147 else if(numA == BANDED)
148 return (BandedMat)(*A.banded() / a);
149 else
150 {
151 THROW_EXCEPTION("invalid type of matrix");
152 }
153 }
154
155 // const SimpleMatrix operator + (const SimpleMatrix& A, const SimpleMatrix& B){
156 // return (DenseMat)(*A.dense() + *B.dense());
157 // }
operator +(const SimpleMatrix & A,const SimpleMatrix & B)158 SimpleMatrix operator + (const SimpleMatrix& A, const SimpleMatrix& B)
159 {
160
161 return (DenseMat)(*A.dense() + *B.dense());
162 }
163
operator +=(SP::SiconosMatrix A,SP::SimpleMatrix B)164 void operator +=(SP::SiconosMatrix A, SP::SimpleMatrix B)
165 {
166 *A += *B;
167 }
168
169
operator +(const SP::SimpleMatrix A,const SP::SimpleMatrix B)170 SP::SimpleMatrix operator +(const SP::SimpleMatrix A, const SP::SimpleMatrix B)
171 {
172 return SP::SimpleMatrix(new SimpleMatrix(*A + *B));
173 }
174
175
176
operator +(const SiconosMatrix & A,const SiconosMatrix & B)177 const SimpleMatrix operator + (const SiconosMatrix& A, const SiconosMatrix& B)
178 {
179 // To compute C = A + B
180
181 if((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
182 THROW_EXCEPTION("inconsistent sizes");
183
184 Siconos::UBLAS_TYPE numA = A.num();
185 Siconos::UBLAS_TYPE numB = B.num();
186
187 // == A or B equal to null ==
188 if(numA == ZERO) // A = 0
189 {
190 if(numB == ZERO) // B = 0
191 return SimpleMatrix(A.size(0), A.size(1));
192 else
193 return SimpleMatrix(B);
194 }
195
196 if(numB == ZERO)
197 return SimpleMatrix(A);
198
199 // == A and B different from 0 ==
200
201 if(numA == numB && numA != 0) // all matrices are of the same type and NOT block
202 {
203 if(numA == DENSE)
204 return (DenseMat)(*A.dense() + *B.dense());
205 else if(numA == TRIANGULAR)
206 return (TriangMat)(*A.triang() + *B.triang());
207 else if(numA == SYMMETRIC)
208 return (SymMat)(*A.sym() + *B.sym());
209 else if(numA == SPARSE)
210 {
211 SparseMat tmp(*A.sparse());
212 tmp += *B.sparse();
213 return tmp;
214 // return (SparseMat)(*A.sparse() + *B.sparse());
215 }
216 else if(numA == SPARSE_COORDINATE)
217 {
218 SparseMat tmp(*A.sparseCoordinate());
219 tmp += *B.sparseCoordinate();
220 return tmp;
221 }
222 else if(numA == BANDED)
223 {
224 BandedMat tmp(*A.banded());
225 tmp += *B.banded();
226 return tmp;
227 }
228 else
229 THROW_EXCEPTION("invalid type of matrix");
230 }
231 else if(numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
232 {
233 if(numA == DENSE)
234 {
235 if(numB == TRIANGULAR)
236 return (DenseMat)(*A.dense() + *B.triang());
237 else if(numB == SYMMETRIC)
238 return (DenseMat)(*A.dense() + *B.sym());
239 else if(numB == SPARSE)
240 return (DenseMat)(*A.dense() + *B.sparse());
241 else if(numB == SPARSE_COORDINATE)
242 return (DenseMat)(*A.dense() + *B.sparseCoordinate());
243 else if(numB == BANDED)
244 return (DenseMat)(*A.dense() + *B.banded());
245 else if(numB == IDENTITY)
246 return (DenseMat)(*A.dense() + *B.identity());
247 else
248 THROW_EXCEPTION("invalid type of matrix");
249 }
250 else if(numA == TRIANGULAR)
251 {
252 if(numB == DENSE)
253 return (DenseMat)(*A.triang() + *B.dense());
254 else if(numB == SYMMETRIC)
255 return (DenseMat)(*A.triang() + *B.sym());
256 else if(numB == SPARSE)
257 return (DenseMat)(*A.triang() + *B.sparse());
258 else if(numB == SPARSE_COORDINATE)
259 return (DenseMat)(*A.triang() + *B.sparseCoordinate());
260 else if(numB == BANDED)
261 return (DenseMat)(*A.triang() + *B.banded());
262 else if(numB == IDENTITY)
263 return (DenseMat)(*A.triang() + *B.identity());
264 else
265 THROW_EXCEPTION("invalid type of matrix");
266 }
267 else if(numA == SYMMETRIC)
268 {
269 if(numB == DENSE)
270 return (DenseMat)(*A.sym() + *B.dense());
271 else if(numB == TRIANGULAR)
272 return (DenseMat)(*A.sym() + *B.triang());
273 else if(numB == SPARSE)
274 return (DenseMat)(*A.sym() + *B.sparse());
275 else if(numB == SPARSE_COORDINATE)
276 return (DenseMat)(*A.sym() + *B.sparseCoordinate());
277 else if(numB == BANDED)
278 return (DenseMat)(*A.sym() + *B.banded());
279 else if(numB == IDENTITY)
280 return (DenseMat)(*A.sym() + *B.identity());
281 else
282 THROW_EXCEPTION("invalid type of matrix");
283 }
284 else if(numA == SPARSE)
285 {
286 if(numB == DENSE)
287 return (DenseMat)(*A.sparse() + *B.dense());
288 else if(numB == TRIANGULAR)
289 return (DenseMat)(*A.sparse() + *B.triang());
290 else if(numB == SYMMETRIC)
291 return (DenseMat)(*A.sparse() + *B.sym());
292 else if(numB == BANDED)
293 return (DenseMat)(*A.sparse() + *B.banded());
294 else if(numB ==IDENTITY)
295 return (DenseMat)(*A.sparse() + *B.identity());
296 else
297 THROW_EXCEPTION("invalid type of matrix");
298 }
299
300 else if(numA == BANDED)
301 {
302 if(numB == DENSE)
303 return (DenseMat)(*A.banded() + *B.dense());
304 else if(numB == TRIANGULAR)
305 return (DenseMat)(*A.banded() + *B.triang());
306 else if(numB == SYMMETRIC)
307 return (DenseMat)(*A.banded() + *B.sym());
308 else if(numB == SPARSE)
309 return (DenseMat)(*A.banded() + *B.sparse());
310 else if(numB == SPARSE_COORDINATE)
311 return (DenseMat)(*A.banded() + *B.sparseCoordinate());
312 else if(numB ==IDENTITY)
313 return (DenseMat)(*A.banded() + *B.identity());
314 else
315 {
316 THROW_EXCEPTION("invalid type of matrix");
317 }
318 }
319
320 else if(numA == IDENTITY)
321 {
322 if(numB == DENSE)
323 return (DenseMat)(*A.identity() + *B.dense());
324 else if(numB == TRIANGULAR)
325 return (DenseMat)(*A.identity() + *B.triang());
326 else if(numB == SYMMETRIC)
327 return (DenseMat)(*A.identity() + *B.sym());
328 else if(numB == SPARSE)
329 return (DenseMat)(*A.identity() + *B.sparse());
330 else if(numB == SPARSE_COORDINATE)
331 return (DenseMat)(*A.identity() + *B.sparseCoordinate());
332 else if(numB == BANDED)
333 return (DenseMat)(*A.identity() + *B.banded());
334 }
335 else
336 THROW_EXCEPTION("invalid type of matrix");
337 }
338 else if(numB != 0) // B Simple, whatever is A
339 {
340 SimpleMatrix tmp(B);
341 tmp += A;
342 return tmp;
343 }
344 else // B Block, A simple or block
345 {
346 SimpleMatrix tmp(A);
347 tmp += B;
348 return tmp;
349 }
350 THROW_EXCEPTION("invalid type of matrix");
351
352 }
353
operator -(const SiconosMatrix & A,const SiconosMatrix & B)354 const SimpleMatrix operator - (const SiconosMatrix& A, const SiconosMatrix& B)
355 {
356 // To compute C = A - B
357
358 if((A.size(0) != B.size(0)) || (A.size(1) != B.size(1)))
359 THROW_EXCEPTION("inconsistent sizes");
360
361 Siconos::UBLAS_TYPE numA = A.num();
362 Siconos::UBLAS_TYPE numB = B.num();
363
364
365 // == B equal to null ==
366 if(numB == ZERO)
367 return SimpleMatrix(A);
368
369 // == B different from 0 ==
370
371 if(numA == numB && numA != 0) // all matrices are of the same type and NOT block
372 {
373 if(numA == DENSE)
374 return (DenseMat)(*A.dense() - *B.dense());
375 else if(numA == TRIANGULAR)
376 return (TriangMat)(*A.triang() - *B.triang());
377 else if(numA == SYMMETRIC)
378 return (SymMat)(*A.sym() - *B.sym());
379 else if(numA == SPARSE)
380 {
381 SparseMat tmp(*A.sparse());
382 tmp -= *B.sparse();
383 return tmp;
384 //return (SparseMat)(*A.sparse() - *B.sparse());
385 }
386 else if(numA==SPARSE_COORDINATE)
387 {
388 SparseCoordinateMat tmp(*A.sparseCoordinate());
389 tmp -= *B.sparseCoordinate();
390 return tmp;
391 }
392 else if(numA == BANDED)
393 {
394 BandedMat tmp(*A.banded());
395 tmp -= *B.banded();
396 return tmp;
397 //return (BandedMat)(*A.banded() - *B.banded());
398 }
399 else
400 {
401 THROW_EXCEPTION("invalid type of matrix");
402 }
403 }
404 else if(numA != 0 && numB != 0 && numA != numB) // A and B of different types and none is block
405 {
406 if(numA == DENSE)
407 {
408 if(numB == TRIANGULAR)
409 return (DenseMat)(*A.dense() - *B.triang());
410 else if(numB == SYMMETRIC)
411 return (DenseMat)(*A.dense() - *B.sym());
412 else if(numB == SPARSE)
413 return (DenseMat)(*A.dense() - *B.sparse());
414 else if(numB ==SPARSE_COORDINATE)
415 return (DenseMat)(*A.dense() - *B.sparseCoordinate());
416 else if(numB == BANDED)
417 return (DenseMat)(*A.dense() - *B.banded());
418 else if(numB == IDENTITY)
419 return (DenseMat)(*A.dense() - *B.identity());
420 else
421 {
422 THROW_EXCEPTION("invalid type of matrix");
423 }
424 }
425 else if(numA == TRIANGULAR)
426 {
427 if(numB == DENSE)
428 return (DenseMat)(*A.triang() - *B.dense());
429 else if(numB == SYMMETRIC)
430 return (DenseMat)(*A.triang() - *B.sym());
431 else if(numB == SPARSE)
432 return (DenseMat)(*A.triang() - *B.sparse());
433 else if(numB ==SPARSE_COORDINATE)
434 return (DenseMat)(*A.triang() - *B.sparseCoordinate());
435 else if(numB == BANDED)
436 return (DenseMat)(*A.triang() - *B.banded());
437 else if(numB == IDENTITY)
438 return (DenseMat)(*A.triang() - *B.identity());
439 else
440 {
441 THROW_EXCEPTION("invalid type of matrix");
442 }
443 }
444 else if(numA == SYMMETRIC)
445 {
446 if(numB == DENSE)
447 return (DenseMat)(*A.sym() - *B.dense());
448 else if(numB == TRIANGULAR)
449 return (DenseMat)(*A.sym() - *B.triang());
450 else if(numB == SPARSE)
451 return (DenseMat)(*A.sym() - *B.sparse());
452 else if(numB ==SPARSE_COORDINATE)
453 return (DenseMat)(*A.sym() - *B.sparseCoordinate());
454 else if(numB == BANDED)
455 return (DenseMat)(*A.sym() - *B.banded());
456 else if(numB == IDENTITY)
457 return (DenseMat)(*A.sym() - *B.identity());
458 else
459 {
460 THROW_EXCEPTION("invalid type of matrix");
461 }
462 }
463 else if(numA == SPARSE)
464 {
465 if(numB == DENSE)
466 return (DenseMat)(*A.sparse() - *B.dense());
467 else if(numB == TRIANGULAR)
468 return (DenseMat)(*A.sparse() - *B.triang());
469 else if(numB == SYMMETRIC)
470 return (DenseMat)(*A.sparse() - *B.sym());
471 else if(numB ==SPARSE_COORDINATE)
472 return (DenseMat)(*A.sparse() - *B.sparseCoordinate());
473 else if(numB == BANDED)
474 return (DenseMat)(*A.sparse() - *B.banded());
475 else if(numB == IDENTITY)
476 return (DenseMat)(*A.sparse() - *B.identity());
477 else
478 {
479 THROW_EXCEPTION("invalid type of matrix");
480 }
481 }
482
483 else if(numA == BANDED)
484 {
485 if(numB == DENSE)
486 return (DenseMat)(*A.banded() - *B.dense());
487 else if(numB == TRIANGULAR)
488 return (DenseMat)(*A.banded() - *B.triang());
489 else if(numB == SYMMETRIC)
490 return (DenseMat)(*A.banded() - *B.sym());
491 else if(numB == SPARSE)
492 return (DenseMat)(*A.banded() - *B.sparse());
493 else if(numB ==SPARSE_COORDINATE)
494 return (DenseMat)(*A.banded() - *B.sparseCoordinate());
495 else if(numB == IDENTITY)
496 return (DenseMat)(*A.banded() - *B.identity());
497 else
498 {
499 THROW_EXCEPTION("invalid type of matrix");
500 }
501 }
502
503 else if(numA == ZERO)
504 {
505 if(numB == DENSE)
506 return (DenseMat)(*A.zero_mat() - *B.dense());
507 else if(numB == TRIANGULAR)
508 return (DenseMat)(*A.zero_mat() - *B.triang());
509 else if(numB == SYMMETRIC)
510 return (DenseMat)(*A.zero_mat() - *B.sym());
511 else if(numB == SPARSE)
512 return (DenseMat)(*A.zero_mat() - *B.sparse());
513 else if(numB ==SPARSE_COORDINATE)
514 return (DenseMat)(*A.zero_mat() - *B.sparseCoordinate());
515 else if(numB == IDENTITY)
516 return (DenseMat)(*A.zero_mat() - *B.identity());
517 else
518 {
519 THROW_EXCEPTION("invalid type of matrix");
520 }
521 }
522 else if(numA == IDENTITY)
523 {
524 if(numB == DENSE)
525 return (DenseMat)(*A.identity() - *B.dense());
526 else if(numB == TRIANGULAR)
527 return (DenseMat)(*A.identity() - *B.triang());
528 else if(numB == SYMMETRIC)
529 return (DenseMat)(*A.identity() - *B.sym());
530 else if(numB == SPARSE)
531 return (DenseMat)(*A.identity() - *B.sparse());
532 else if(numB ==SPARSE_COORDINATE)
533 return (DenseMat)(*A.identity() - *B.sparseCoordinate());
534 else if(numB == BANDED)
535 return (DenseMat)(*A.identity() - *B.banded());
536 else
537 {
538 THROW_EXCEPTION("invalid type of matrix");
539 }
540 }
541 else
542 {
543 THROW_EXCEPTION("invalid type of matrix");
544 }
545 }
546 else // A and/or B are/is Block
547 {
548 SimpleMatrix tmp(A);
549 tmp -= B;
550 return tmp;
551 }
552
553
554 }
555
556 //========================
557 // Matrices comparison
558 //========================
559
operator ==(const SiconosMatrix & m,const SiconosMatrix & x)560 bool operator == (const SiconosMatrix &m, const SiconosMatrix &x)
561 {
562 // if( ! isComparableTo( m, x))
563 // return false;
564 // Warning : two block matrices may be "equal" but have blocks of different sizes.
565 double norm = (m - x).normInf();
566 DEBUG_EXPR((m - x).display());
567 DEBUG_PRINTF("norm = %12.8e \n", norm);
568 DEBUG_PRINTF("std::numeric_limits<double>::epsilon() = %12.8e \n", std::numeric_limits<double>::epsilon());
569 DEBUG_EXPR(std::cout << std::boolalpha << (norm <= std::numeric_limits<double>::epsilon()) <<std::endl;);
570 double atol = 1e-14;
571 double rtol = std::numeric_limits<double>::epsilon();
572 return (norm <= atol + rtol * x.normInf()) ;
573 }
574
operator !=(const SiconosMatrix & m,const SiconosMatrix & x)575 bool operator != (const SiconosMatrix &m, const SiconosMatrix &x)
576 {
577 double norm = (m - x).normInf();
578 return (norm > std::numeric_limits<double>::epsilon());
579 }
580