1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/naivemh.cc,v 1.40 2017/01/12 14:43:54 masarati Exp $ */
2 /*
3 * MBDyn (C) is a multibody analysis code.
4 * http://www.mbdyn.org
5 *
6 * Copyright (C) 2003-2017
7 *
8 * This code is a partial merge of HmFe and MBDyn.
9 *
10 * Pierangelo Masarati <masarati@aero.polimi.it>
11 * Paolo Mantegazza <mantegazza@aero.polimi.it>
12 *
13 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
14 * via La Masa, 34 - 20156 Milano, Italy
15 * http://www.aero.polimi.it
16 *
17 * Changing this copyright notice is forbidden.
18 *
19 * This program is free software; you can redistribute it and/or modify
20 * it under the terms of the GNU General Public License as published by
21 * the Free Software Foundation (version 2 of the License).
22 *
23 *
24 * This program is distributed in the hope that it will be useful,
25 * but WITHOUT ANY WARRANTY; without even the implied warranty of
26 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
27 * GNU General Public License for more details.
28 *
29 * You should have received a copy of the GNU General Public License
30 * along with this program; if not, write to the Free Software
31 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
32 */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include <cstring>
37 #include <cassert>
38 #include "myassert.h"
39 #include "mh.h"
40 #include "submat.h"
41 #include "naivemh.h"
42 #include "mthrdslv.h"
43
44 #undef CHECK_FOR_ZERO
45
46 /* NaiveMatrixHandler begin */
47
NaiveMatrixHandler(const integer n,NaiveMatrixHandler * const nmh)48 NaiveMatrixHandler::NaiveMatrixHandler(const integer n,
49 NaiveMatrixHandler *const nmh)
50 : iSize(n), bOwnsMemory(true),
51 ppdRows(0), ppiRows(0), ppiCols(0), ppnonzero(0), piNzr(0), piNzc(0), m_end(*this, true)
52 {
53 if (nmh) {
54 bOwnsMemory = false;
55 iSize = nmh->iSize;
56 ppdRows = nmh->ppdRows;
57 ppiRows = nmh->ppiRows;
58 ppiCols = nmh->ppiCols;
59 ppnonzero = nmh->ppnonzero;
60 piNzr = nmh->piNzr;
61 piNzc = nmh->piNzc;
62
63 } else {
64 ASSERT(iSize > 0);
65
66 SAFENEWARR(ppiRows, integer *, iSize);
67 ppiRows[0] = 0;
68 SAFENEWARR(ppiRows[0], integer, iSize*iSize);
69
70 SAFENEWARR(ppiCols, integer *, iSize);
71 ppiCols[0] = 0;
72 SAFENEWARR(ppiCols[0], integer, iSize*iSize);
73
74 SAFENEWARR(ppdRows, doublereal *, iSize);
75 ppdRows[0] = NULL;
76 SAFENEWARR(ppdRows[0], doublereal, iSize*iSize);
77
78 SAFENEWARR(ppnonzero, char *, iSize);
79 ppnonzero[0] = NULL;
80 SAFENEWARR(ppnonzero[0], char, iSize*iSize);
81
82 for (integer i = 1; i < iSize; i++) {
83 ppiRows[i] = ppiRows[i - 1] + iSize;
84 ppiCols[i] = ppiCols[i - 1] + iSize;
85 ppdRows[i] = ppdRows[i - 1] + iSize;
86 ppnonzero[i] = ppnonzero[i - 1] + iSize;
87 }
88
89 SAFENEWARR(piNzr, integer, iSize);
90 SAFENEWARR(piNzc, integer, iSize);
91
92 #ifdef HAVE_MEMSET
93 memset(ppdRows[0], 0, sizeof(doublereal)*iSize*iSize);
94 memset(ppnonzero[0], 0, sizeof(char)*iSize*iSize);
95 memset(piNzr, 0, sizeof(integer)*iSize);
96 memset(piNzc, 0, sizeof(integer)*iSize);
97 #else /* ! HAVE_MEMSET */
98 for (integer row = 0; row < iSize; row++) {
99 for (integer col = 0; col < iSize; col++) {
100 ppnonzero[row][col] = 0;
101 }
102 piNzr[row] = 0;
103 piNzc[row] = 0;
104 }
105 #endif /* ! HAVE_MEMSET */
106 }
107 }
108
~NaiveMatrixHandler(void)109 NaiveMatrixHandler::~NaiveMatrixHandler(void)
110 {
111 if (bOwnsMemory) {
112 if (ppiRows) {
113 if (ppiRows[0]) {
114 SAFEDELETEARR(ppiRows[0]);
115 }
116 SAFEDELETEARR(ppiRows);
117 }
118
119 if (ppiCols) {
120 if (ppiCols[0]) {
121 SAFEDELETEARR(ppiCols[0]);
122 }
123 SAFEDELETEARR(ppiCols);
124 }
125
126 if (ppdRows) {
127 if (ppdRows[0]) {
128 SAFEDELETEARR(ppdRows[0]);
129 }
130 SAFEDELETEARR(ppdRows);
131 }
132
133 if (ppnonzero) {
134 if (ppnonzero[0]) {
135 SAFEDELETEARR(ppnonzero[0]);
136 }
137 SAFEDELETEARR(ppnonzero);
138 }
139
140 if (piNzr) {
141 SAFEDELETEARR(piNzr);
142 }
143
144 if (piNzc) {
145 SAFEDELETEARR(piNzc);
146 }
147 }
148 }
149
150 void
Reset(void)151 NaiveMatrixHandler::Reset(void)
152 {
153 for (integer row = 0; row < iSize; row++) {
154 integer ncols = piNzc[row];
155 integer *piCols = ppiCols[row];
156 char *pnonzero = ppnonzero[row];
157 for (integer col = 0; col < ncols; col++) {
158 pnonzero[piCols[col]] = 0;
159 }
160
161 piNzr[row] = 0;
162 piNzc[row] = 0;
163 }
164 }
165
166 /* Overload di += usato per l'assemblaggio delle matrici */
167 MatrixHandler&
operator +=(const SubMatrixHandler & SubMH)168 NaiveMatrixHandler::operator += (const SubMatrixHandler& SubMH)
169 {
170 integer nr = SubMH.iGetNumRows();
171 integer nc = SubMH.iGetNumCols();
172
173 for (integer ir = 1; ir <= nr; ir++) {
174 integer iRow = SubMH.iGetRowIndex(ir);
175
176 for (integer ic = 1; ic <= nc; ic++) {
177 doublereal d = SubMH(ir, ic);
178
179 #ifdef CHECK_FOR_ZERO
180 if (d != 0.)
181 #endif /* CHECK_FOR_ZERO */
182 {
183 integer iCol = SubMH.iGetColIndex(ic);
184
185 operator()(iRow, iCol) += d;
186 }
187 }
188 }
189
190 return *this;
191 }
192
193 /* Overload di -= usato per l'assemblaggio delle matrici */
194 MatrixHandler&
operator -=(const SubMatrixHandler & SubMH)195 NaiveMatrixHandler::operator -= (const SubMatrixHandler& SubMH)
196 {
197 integer nr = SubMH.iGetNumRows();
198 integer nc = SubMH.iGetNumCols();
199
200 for (integer ir = 1; ir <= nr; ir++) {
201 integer iRow = SubMH.iGetRowIndex(ir);
202
203 for (integer ic = 1; ic <= nc; ic++) {
204 doublereal d = SubMH(ir, ic);
205
206 #ifdef CHECK_FOR_ZERO
207 if (d != 0.)
208 #endif /* CHECK_FOR_ZERO */
209 {
210 integer iCol = SubMH.iGetColIndex(ic);
211
212 operator()(iRow, iCol) -= d;
213 }
214 }
215 }
216
217 return *this;
218 }
219
220 /* Overload di += usato per l'assemblaggio delle matrici
221 * questi li vuole ma non so bene perche'; force per la doppia
222 * derivazione di VariableSubMatrixHandler */
223 MatrixHandler&
operator +=(const VariableSubMatrixHandler & SubMH)224 NaiveMatrixHandler::operator += (const VariableSubMatrixHandler& SubMH)
225 {
226 switch (SubMH.eStatus) {
227 case VariableSubMatrixHandler::FULL:
228 {
229 const FullSubMatrixHandler& SMH =
230 *dynamic_cast<const FullSubMatrixHandler *>(&SubMH);
231 /* NOTE: pirm1 is 1-based, for optimization purposes */
232 integer *pirm1 = SMH.piRowm1;
233 /* NOTE: pic is 0-based, for optimization purposes */
234 integer *pic = SMH.piColm1 + 1;
235
236 /* NOTE: ppd is 1-based for rows; access to SMH(iRow, iCol)
237 * results in ppd[iCol - 1][iRow] */
238 doublereal **ppd = SMH.ppdCols;
239
240 integer nr = SMH.iGetNumRows();
241 integer nc = SMH.iGetNumCols();
242 /* NOTE: iR is 1-based, for optimization purposes */
243 for (integer iR = 1; iR <= nr; iR++) {
244 integer iRow = pirm1[iR];
245
246 /* NOTE: ic is 0-based, for optimization purposes */
247 for (integer ic = 0; ic < nc; ic++) {
248 doublereal d = ppd[ic][iR];
249
250 #ifdef CHECK_FOR_ZERO
251 if (d != 0.)
252 #endif /* CHECK_FOR_ZERO */
253 {
254 integer iCol = pic[ic];
255
256 operator()(iRow, iCol) += d;
257 }
258 }
259 }
260 break;
261 }
262
263 case VariableSubMatrixHandler::SPARSE:
264 {
265 const SparseSubMatrixHandler& SMH =
266 *dynamic_cast<const SparseSubMatrixHandler *>(&SubMH);
267
268 for (integer i = 1; i <= SMH.iNumItems; i++) {
269 doublereal d = SMH.pdMatm1[i];
270
271 #ifdef CHECK_FOR_ZERO
272 if (d != 0.)
273 #endif /* CHECK_FOR_ZERO */
274 {
275 integer iRow = SMH.piRowm1[i];
276 integer iCol = SMH.piColm1[i];
277
278 operator()(iRow, iCol) += d;
279 }
280 }
281 break;
282 }
283
284 default:
285 break;
286 }
287
288 return *this;
289 }
290
291 MatrixHandler&
operator -=(const VariableSubMatrixHandler & SubMH)292 NaiveMatrixHandler::operator -= (const VariableSubMatrixHandler& SubMH)
293 {
294 switch (SubMH.eStatus) {
295 case VariableSubMatrixHandler::FULL:
296 {
297 const FullSubMatrixHandler& SMH =
298 *dynamic_cast<const FullSubMatrixHandler *>(&SubMH);
299 integer *pirm1 = SMH.piRowm1;
300 integer *pic = SMH.piColm1 + 1;
301 doublereal **ppd = SMH.ppdCols;
302
303 integer nr = SMH.iGetNumRows();
304 integer nc = SMH.iGetNumCols();
305 for (integer iR = 1; iR <= nr; iR++) {
306 integer iRow = pirm1[iR];
307
308 for (integer ic = 0; ic < nc; ic++) {
309 doublereal d = ppd[ic][iR];
310
311 #ifdef CHECK_FOR_ZERO
312 if (d != 0.)
313 #endif /* CHECK_FOR_ZERO */
314 {
315 integer iCol = pic[ic];
316
317 operator()(iRow, iCol) -= d;
318 }
319 }
320 }
321 break;
322 }
323
324 case VariableSubMatrixHandler::SPARSE:
325 {
326 const SparseSubMatrixHandler& SMH =
327 *dynamic_cast<const SparseSubMatrixHandler *>(&SubMH);
328
329 for (integer i = 1; i <= SMH.iNumItems; i++) {
330 doublereal d = SMH.pdMatm1[i];
331
332 #ifdef CHECK_FOR_ZERO
333 if (d != 0.)
334 #endif /* CHECK_FOR_ZERO */
335 {
336 integer iRow = SMH.piRowm1[i];
337 integer iCol = SMH.piColm1[i];
338
339 operator()(iRow, iCol) -= d;
340 }
341 }
342 break;
343 }
344
345 default:
346 break;
347 }
348
349 return *this;
350 }
351
352 void
MakeCCStructure(std::vector<integer> & Ai,std::vector<integer> & Ap)353 NaiveMatrixHandler::MakeCCStructure(std::vector<integer>& Ai,
354 std::vector<integer>& Ap) {
355 integer nnz = 0;
356 for (integer i = 0; i < iSize; i++) {
357 nnz += piNzr[i];
358 }
359 Ai.resize(nnz);
360 Ap.resize(iSize + 1);
361 integer x_ptr = 0;
362 for (integer col = 0; col < iSize; col++) {
363 Ap[col] = x_ptr;
364 integer nzr = piNzr[col];
365 for (integer row = 0; row < nzr; row++) {
366 Ai[x_ptr] = ppiRows[col][row];
367 x_ptr++;
368 }
369 }
370 Ap[iSize] = nnz;
371 };
372
373 /* Matrix Matrix product */
374 MatrixHandler&
MatMatMul_base(void (MatrixHandler::* op)(integer iRow,integer iCol,const doublereal & dCoef),MatrixHandler & out,const MatrixHandler & in) const375 NaiveMatrixHandler::MatMatMul_base(
376 void (MatrixHandler::*op)(integer iRow, integer iCol, const doublereal& dCoef),
377 MatrixHandler& out, const MatrixHandler& in) const
378 {
379 ASSERT(in.iGetNumRows() == iSize);
380 ASSERT(out.iGetNumRows() == iSize);
381 ASSERT(out.iGetNumCols() == in.iGetNumCols());
382
383 integer in_ncols = in.iGetNumCols();
384
385 for (integer ir = 0; ir < iSize; ir++) {
386 for (integer idx = 0; idx < piNzc[ir]; idx++) {
387 integer ic = ppiCols[ir][idx];
388 for (integer ik = 1; ik <= in_ncols; ik++) {
389 (out.*op)(ir + 1, ik, ppdRows[ir][ic]*in(ic + 1, ik));
390 }
391 }
392 }
393
394 return out;
395 }
396
397 MatrixHandler&
MatTMatMul_base(void (MatrixHandler::* op)(integer iRow,integer iCol,const doublereal & dCoef),MatrixHandler & out,const MatrixHandler & in) const398 NaiveMatrixHandler::MatTMatMul_base(
399 void (MatrixHandler::*op)(integer iRow, integer iCol, const doublereal& dCoef),
400 MatrixHandler& out, const MatrixHandler& in) const
401 {
402 ASSERT(in.iGetNumRows() == iSize);
403 ASSERT(out.iGetNumRows() == iSize);
404 ASSERT(out.iGetNumCols() == in.iGetNumCols());
405
406 integer in_ncols = in.iGetNumCols();
407
408 for (integer ic = 0; ic < iSize; ic++) {
409 for (integer idx = 0; idx < piNzr[ic]; idx++) {
410 integer ir = ppiRows[ic][idx];
411 for (integer ik = 1; ik <= in_ncols; ik++) {
412 (out.*op)(ic + 1, ik, ppdRows[ir][ic]*in(ir + 1, ik));
413 }
414 }
415 }
416
417 return out;
418 }
419
420 /* Matrix Vector product */
421 VectorHandler&
MatVecMul_base(void (VectorHandler::* op)(integer iRow,const doublereal & dCoef),VectorHandler & out,const VectorHandler & in) const422 NaiveMatrixHandler::MatVecMul_base(
423 void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
424 VectorHandler& out, const VectorHandler& in) const
425 {
426 ASSERT(in.iGetSize() == iSize);
427 ASSERT(out.iGetSize() == iSize);
428
429 for (integer ir = 0; ir < iSize; ir++) {
430 for (integer idx = 0; idx < piNzc[ir]; idx++) {
431 integer ic = ppiCols[ir][idx];
432 (out.*op)(ir + 1, ppdRows[ir][ic]*in(ic + 1));
433 }
434 }
435
436 return out;
437 }
438
439 VectorHandler&
MatTVecMul_base(void (VectorHandler::* op)(integer iRow,const doublereal & dCoef),VectorHandler & out,const VectorHandler & in) const440 NaiveMatrixHandler::MatTVecMul_base(
441 void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
442 VectorHandler& out, const VectorHandler& in) const
443 {
444 ASSERT(in.iGetSize() == iSize);
445 ASSERT(out.iGetSize() == iSize);
446
447 for (integer ic = 0; ic < iSize; ic++) {
448 for (integer idx = 0; idx < piNzr[ic]; idx++) {
449 integer ir = ppiRows[ic][idx];
450 (out.*op)(ic + 1, ppdRows[ir][ic]*in(ir + 1));
451 }
452 }
453
454 return out;
455 }
456
457 void
reset(bool is_end)458 NaiveMatrixHandler::const_iterator::reset(bool is_end)
459 {
460 if (is_end) {
461 elem.iRow = m.iSize;
462 elem.iCol = m.iSize;
463
464 } else {
465 i_row = 0;
466 elem.iCol = 0;
467
468 while (m.piNzr[elem.iCol] == 0) {
469 if (++elem.iCol == m.iSize) {
470 elem.iRow = m.iSize;
471 return;
472 }
473 }
474
475 elem.iRow = m.ppiRows[elem.iCol][i_row];
476 elem.dCoef = m.ppdRows[elem.iRow][elem.iCol];
477 }
478 }
479
const_iterator(const NaiveMatrixHandler & m,bool is_end)480 NaiveMatrixHandler::const_iterator::const_iterator(const NaiveMatrixHandler& m, bool is_end)
481 : m(m)
482 {
483 reset(is_end);
484 }
485
~const_iterator(void)486 NaiveMatrixHandler::const_iterator::~const_iterator(void)
487 {
488 NO_OP;
489 }
490
491 const NaiveMatrixHandler::const_iterator&
operator ++(void) const492 NaiveMatrixHandler::const_iterator::operator ++ (void) const
493 {
494 i_row++;
495 while (i_row == m.piNzr[elem.iCol]) {
496 if (++elem.iCol == m.iSize) {
497 elem.iRow = m.iSize;
498 return *this;
499 }
500
501 i_row = 0;
502 }
503
504 elem.iRow = m.ppiRows[elem.iCol][i_row];
505 elem.dCoef = m.ppdRows[elem.iRow][elem.iCol];
506
507 return *this;
508 }
509
510 const SparseMatrixHandler::SparseMatrixElement *
operator ->(void) const511 NaiveMatrixHandler::const_iterator::operator -> (void) const
512 {
513 return &elem;
514 }
515
516 const SparseMatrixHandler::SparseMatrixElement&
operator *(void) const517 NaiveMatrixHandler::const_iterator::operator * (void) const
518 {
519 return elem;
520 }
521
522 bool
operator ==(const NaiveMatrixHandler::const_iterator & op) const523 NaiveMatrixHandler::const_iterator::operator == (const NaiveMatrixHandler::const_iterator& op) const
524 {
525 return elem == op.elem;
526 }
527
528 bool
operator !=(const NaiveMatrixHandler::const_iterator & op) const529 NaiveMatrixHandler::const_iterator::operator != (const NaiveMatrixHandler::const_iterator& op) const
530 {
531 return elem != op.elem;
532 }
533
534 /* NaiveMatrixHandler end */
535
536 /* NaivePermMatrixHandler begin */
537
NaivePermMatrixHandler(integer iSize,const std::vector<integer> & tperm,const std::vector<integer> & tinvperm)538 NaivePermMatrixHandler::NaivePermMatrixHandler(integer iSize,
539 const std::vector<integer>& tperm,
540 const std::vector<integer>& tinvperm)
541 : NaiveMatrixHandler(iSize), perm(tperm), invperm(tinvperm), m_end(*this, true)
542 {
543 ASSERT(perm.size() == (size_t)iSize);
544 ASSERT(invperm.size() == (size_t)iSize);
545
546 #ifdef DEBUG
547 for (integer i = 0; i < iSize; ++i) {
548 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
549 ASSERT(perm[i] >= 0 && perm[i] < iSize);
550 }
551 #endif
552 }
553
NaivePermMatrixHandler(NaiveMatrixHandler * const nmh,const std::vector<integer> & tperm,const std::vector<integer> & tinvperm)554 NaivePermMatrixHandler::NaivePermMatrixHandler(NaiveMatrixHandler *const nmh,
555 const std::vector<integer>& tperm,
556 const std::vector<integer>& tinvperm)
557 : NaiveMatrixHandler(0, nmh), perm(tperm), invperm(tinvperm), m_end(*this, true)
558 {
559 ASSERT(perm.size() == (size_t)iSize);
560 ASSERT(invperm.size() == (size_t)iSize);
561
562 #ifdef DEBUG
563 for (integer i = 0; i < iSize; ++i) {
564 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
565 ASSERT(perm[i] >= 0 && perm[i] < iSize);
566 }
567 #endif
568
569 NO_OP;
570 }
571
~NaivePermMatrixHandler(void)572 NaivePermMatrixHandler::~NaivePermMatrixHandler(void)
573 {
574 DEBUGCOUTFNAME("NaivePermMatrixHandler::~NaivePermMatrixHandler");
575 }
576
577 const std::vector<integer>&
GetPerm(void) const578 NaivePermMatrixHandler::GetPerm(void) const
579 {
580 #ifdef DEBUG
581 ASSERT(perm.size() == (size_t)iSize);
582
583 for (integer i = 0; i < iSize; ++i) {
584 ASSERT(perm[i] >= 0 && perm[i] < iSize);
585 }
586 #endif
587 return perm;
588 }
589
590 const std::vector<integer>&
GetInvPerm(void) const591 NaivePermMatrixHandler::GetInvPerm(void) const
592 {
593 #ifdef DEBUG
594 ASSERT(invperm.size() == (size_t)iSize);
595
596 for (integer i = 0; i < iSize; ++i) {
597 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
598 }
599 #endif
600 return invperm;
601 }
602
603 /* Matrix Matrix product */
604 MatrixHandler&
MatMatMul_base(void (MatrixHandler::* op)(integer iRow,integer iCol,const doublereal & dCoef),MatrixHandler & out,const MatrixHandler & in) const605 NaivePermMatrixHandler::MatMatMul_base(
606 void (MatrixHandler::*op)(integer iRow, integer iCol, const doublereal& dCoef),
607 MatrixHandler& out, const MatrixHandler& in) const
608 {
609 #ifdef DEBUG
610 ASSERT(perm.size() == (size_t)iSize);
611 ASSERT(invperm.size() == (size_t)iSize);
612
613 for (integer i = 0; i < iSize; ++i) {
614 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
615 ASSERT(perm[i] >= 0 && perm[i] < iSize);
616 }
617 #endif
618
619 ASSERT(in.iGetNumRows() == iSize);
620 ASSERT(out.iGetNumRows() == iSize);
621 ASSERT(out.iGetNumCols() == in.iGetNumCols());
622
623 integer in_ncols = in.iGetNumCols();
624
625 for (integer ir = 0; ir < iSize; ir++) {
626 for (integer idx = 0; idx < piNzc[ir]; idx++) {
627 integer ic = ppiCols[ir][idx];
628 for (integer ik = 1; ik <= in_ncols; ik++) {
629 (out.*op)(ir + 1, ik, ppdRows[ir][ic]*in(invperm[ic] + 1, ik));
630 }
631 }
632 }
633
634 return out;
635 }
636
637 MatrixHandler&
MatTMatMul_base(void (MatrixHandler::* op)(integer iRow,integer iCol,const doublereal & dCoef),MatrixHandler & out,const MatrixHandler & in) const638 NaivePermMatrixHandler::MatTMatMul_base(
639 void (MatrixHandler::*op)(integer iRow, integer iCol, const doublereal& dCoef),
640 MatrixHandler& out, const MatrixHandler& in) const
641 {
642 #ifdef DEBUG
643 ASSERT(perm.size() == (size_t)iSize);
644 ASSERT(invperm.size() == (size_t)iSize);
645
646 for (integer i = 0; i < iSize; ++i) {
647 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
648 ASSERT(perm[i] >= 0 && perm[i] < iSize);
649 }
650 #endif
651 ASSERT(in.iGetNumRows() == iSize);
652 ASSERT(out.iGetNumRows() == iSize);
653 ASSERT(out.iGetNumCols() == in.iGetNumCols());
654
655 integer in_ncols = in.iGetNumCols();
656
657 for (integer ic = 0; ic < iSize; ic++) {
658 for (integer idx = 0; idx < piNzr[ic]; idx++) {
659 integer ir = ppiRows[ic][idx];
660 for (integer ik = 1; ik <= in_ncols; ik++) {
661 (out.*op)(invperm[ic] + 1, ik, ppdRows[ir][ic]*in(ir + 1, ik));
662 }
663 }
664 }
665
666 return out;
667 }
668
669 /* Matrix Vector product */
670 VectorHandler&
MatVecMul_base(void (VectorHandler::* op)(integer iRow,const doublereal & dCoef),VectorHandler & out,const VectorHandler & in) const671 NaivePermMatrixHandler::MatVecMul_base(
672 void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
673 VectorHandler& out, const VectorHandler& in) const
674 {
675 #ifdef DEBUG
676 ASSERT(perm.size() == (size_t)iSize);
677 ASSERT(invperm.size() == (size_t)iSize);
678
679 for (integer i = 0; i < iSize; ++i) {
680 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
681 ASSERT(perm[i] >= 0 && perm[i] < iSize);
682 }
683 #endif
684 ASSERT(in.iGetSize() == iSize);
685 ASSERT(out.iGetSize() == iSize);
686
687 for (integer ir = 0; ir < iSize; ir++) {
688 for (integer idx = 0; idx < piNzc[ir]; idx++) {
689 integer ic = ppiCols[ir][idx];
690 (out.*op)(ir + 1, ppdRows[ir][ic]*in(invperm[ic] + 1));
691 }
692 }
693
694 return out;
695 }
696
697 VectorHandler&
MatTVecMul_base(void (VectorHandler::* op)(integer iRow,const doublereal & dCoef),VectorHandler & out,const VectorHandler & in) const698 NaivePermMatrixHandler::MatTVecMul_base(
699 void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
700 VectorHandler& out, const VectorHandler& in) const
701 {
702 #ifdef DEBUG
703 ASSERT(perm.size() == (size_t)iSize);
704 ASSERT(invperm.size() == (size_t)iSize);
705
706 for (integer i = 0; i < iSize; ++i) {
707 ASSERT(invperm[i] >= 0 && invperm[i] < iSize);
708 ASSERT(perm[i] >= 0 && perm[i] < iSize);
709 }
710 #endif
711 ASSERT(in.iGetSize() == iSize);
712 ASSERT(out.iGetSize() == iSize);
713
714 for (integer ic = 0; ic < iSize; ic++) {
715 for (integer idx = 0; idx < piNzr[ic]; idx++) {
716 integer ir = ppiRows[ic][idx];
717 (out.*op)(invperm[ic] + 1, ppdRows[ir][ic]*in(ir + 1));
718 }
719 }
720
721 return out;
722 }
723
724 void
reset(bool is_end)725 NaivePermMatrixHandler::const_iterator::reset(bool is_end)
726 {
727 if (is_end) {
728 elem.iRow = m.iSize;
729 elem.iCol = m.iSize;
730
731 } else {
732 i_row = 0;
733 elem.iCol = 0;
734 elem.iRow = m.ppiRows[m.perm[elem.iCol]][i_row];
735 elem.dCoef = m.ppdRows[elem.iRow][m.perm[elem.iCol]];
736 }
737 }
738
const_iterator(const NaivePermMatrixHandler & m)739 NaivePermMatrixHandler::const_iterator::const_iterator(const NaivePermMatrixHandler& m)
740 : m(m), i_row(0), elem(0, 0, 0.)
741 {
742 while (m.piNzr[m.perm[elem.iCol]] == 0) {
743 if (++elem.iCol == m.iSize) {
744 elem.iRow = m.iSize;
745 return;
746 }
747 }
748
749 elem.iRow = m.ppiRows[m.perm[elem.iCol]][i_row];
750 elem.dCoef = m.ppdRows[elem.iRow][m.perm[elem.iCol]];
751 }
752
const_iterator(const NaivePermMatrixHandler & m,bool)753 NaivePermMatrixHandler::const_iterator::const_iterator(const NaivePermMatrixHandler& m, bool)
754 : m(m), i_row(0), elem(m.iSize, m.iSize, 0.)
755 {
756 NO_OP;
757 }
758
~const_iterator(void)759 NaivePermMatrixHandler::const_iterator::~const_iterator(void)
760 {
761 NO_OP;
762 }
763
764 const NaivePermMatrixHandler::const_iterator&
operator ++(void) const765 NaivePermMatrixHandler::const_iterator::operator ++ (void) const
766 {
767 i_row++;
768 while (i_row == m.piNzr[m.perm[elem.iCol]]) {
769 if (++elem.iCol == m.iSize) {
770 elem.iRow = m.iSize;
771 return *this;
772 }
773
774 i_row = 0;
775 }
776
777 elem.iRow = m.ppiRows[m.perm[elem.iCol]][i_row];
778 elem.dCoef = m.ppdRows[elem.iRow][m.perm[elem.iCol]];
779
780 return *this;
781 }
782
783 const SparseMatrixHandler::SparseMatrixElement *
operator ->(void) const784 NaivePermMatrixHandler::const_iterator::operator -> (void) const
785 {
786 return &elem;
787 }
788
789 const SparseMatrixHandler::SparseMatrixElement&
operator *(void) const790 NaivePermMatrixHandler::const_iterator::operator * (void) const
791 {
792 return elem;
793 }
794
795 bool
operator ==(const NaivePermMatrixHandler::const_iterator & op) const796 NaivePermMatrixHandler::const_iterator::operator == (const NaivePermMatrixHandler::const_iterator& op) const
797 {
798 return elem == op.elem;
799 }
800
801 bool
operator !=(const NaivePermMatrixHandler::const_iterator & op) const802 NaivePermMatrixHandler::const_iterator::operator != (const NaivePermMatrixHandler::const_iterator& op) const
803 {
804 return elem != op.elem;
805 }
806
807 /* NaivePermMatrixHandler end */
808
809