MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
mh.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/mh.cc,v 1.35 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) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* solution manager */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <cstring> /* for memset() */
37 
38 #include <iostream>
39 #include <iomanip>
40 #include <vector>
41 
42 #include "solman.h"
43 #include "submat.h"
44 #include "matvec3.h"
45 #include "ac/lapack.h"
46 
47 /* MatrixHandler - begin */
48 
50 {
51  NO_OP;
52 }
53 
54 /* Ridimensiona ed inizializza. */
55 void
57 {
58  Resize(iNewRow, iNewCol);
59  Reset();
60 }
61 
62 /* Impacchetta la matrice; restituisce il numero di elementi
63  * diversi da zero */
64 integer
66 {
67  return 0L;
68 }
69 
70 /* Overload di = */
73 {
74  integer nr = MH.iGetNumRows();
75  integer nc = MH.iGetNumCols();
76 
77  Resize(nr, nc);
78 
79  for (integer i = 1; i <= nr; i++) {
80  for (integer ii = 1; ii <= nc; ii++) {
81  this->operator()(i, ii) = MH(i, ii);
82  }
83  }
84 
85  return *this;
86 }
87 
88 
89 
90 /* Overload di += usato per l'assemblaggio delle matrici */
93 {
94  return SubMH.AddTo(*this);
95 }
96 
97 /* Overload di -= usato per l'assemblaggio delle matrici */
100 {
101  return SubMH.SubFrom(*this);
102 }
103 
104 /* Overload di += usato per l'assemblaggio delle matrici */
107 {
108  return SubMH.AddTo(*this);
109 }
110 
111 /* Overload di -= usato per l'assemblaggio delle matrici */
114 {
115  return SubMH.SubFrom(*this);
116 }
117 
120 {
121  integer nr = iGetNumRows();
122  integer nc = iGetNumCols();
123 
124  for (integer i = 1; i <= nr; i++) {
125  for (integer j = 1; j <= nc; j++) {
126  this->operator()(i, j) *= d;
127  }
128  }
129 
130  return *this;
131 }
132 
133 /* Matrix Matrix product */
136  integer iCol, const doublereal& dCoef),
137  MatrixHandler& out, const MatrixHandler& in) const
138 {
139  integer out_nc = out.iGetNumCols();
140  integer out_nr = out.iGetNumRows();
141  integer in_nr = in.iGetNumRows();
142 
143  if (out_nr != iGetNumRows()
144  || out_nc != in.iGetNumCols()
145  || in_nr != iGetNumCols())
146  {
147  const char *strop;
148 
149  if (op == &MatrixHandler::IncCoef) {
150  strop = "+=";
151  } else if (op == &MatrixHandler::DecCoef) {
152  strop = "-=";
153  } else if (op == &MatrixHandler::PutCoef) {
154  strop = "=";
155  } else {
157  }
158 
159  silent_cerr("MatrixHandler::MatMatMul_base: size mismatch "
160  "out(" << out_nr << ", " << out_nc << ") "
161  << strop << " this(" << iGetNumRows() << ", " << iGetNumCols() << ") "
162  "* in(" << in_nr << ", " << in.iGetNumCols() << ")"
163  << std::endl);
165  }
166 
167  for (integer c = 1; c <= out_nc; c++) {
168  for (integer r = 1; r <= out_nr; r++) {
169  doublereal d = 0.;
170 
171  for (integer k = 1; k <= in_nr; k++) {
172  d += this->operator()(r, k)*in(k, c);
173  }
174 
175  (out.*op)(r, c, d);
176  }
177  }
178 
179  return out;
180 }
181 
184  integer iCol, const doublereal& dCoef),
185  MatrixHandler& out, const MatrixHandler& in) const
186 {
187  integer out_nc = out.iGetNumCols();
188  integer out_nr = out.iGetNumRows();
189  integer in_nr = in.iGetNumRows();
190 
191  if (out_nr != iGetNumCols()
192  || out_nc != in.iGetNumCols()
193  || in_nr != iGetNumRows())
194  {
195  const char *strop;
196 
197  if (op == &MatrixHandler::IncCoef) {
198  strop = "+=";
199  } else if (op == &MatrixHandler::DecCoef) {
200  strop = "-=";
201  } else if (op == &MatrixHandler::PutCoef) {
202  strop = "=";
203  } else {
205  }
206 
207  silent_cerr("MatrixHandler::MatTMatMul_base: size mismatch "
208  "out(" << out_nr << ", " << out_nc << ") "
209  << strop << " this(" << iGetNumRows() << ", " << iGetNumCols() << ")^T "
210  "* in(" << in_nr << ", " << in.iGetNumCols() << ")"
211  << std::endl);
213  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
214  }
215 
216  for (integer c = 1; c <= out_nc; c++) {
217  for (integer r = 1; r <= out_nr; r++) {
218  doublereal d = 0.;
219 
220  for (integer k = 1; k <= in_nr; k++) {
221  d += this->operator()(k, r)*in(k, c);
222  }
223 
224  (out.*op)(r, c, d);
225  }
226  }
227 
228  return out;
229 }
230 
233 {
234  /* Put is implemented resetting out first, then passing IncCoef()
235  * so that out-of-order assignments work */
236  out.Reset();
237  return MatMatMul_base(&MatrixHandler::IncCoef, out, in);
238 }
239 
242 {
243  /* Put is implemented resetting out first, then passing IncCoef()
244  * so that out-of-order assignments work */
245  out.Reset();
246  return MatTMatMul_base(&MatrixHandler::IncCoef, out, in);
247 }
248 
251 {
252  return MatMatMul_base(&MatrixHandler::IncCoef, out, in);
253 }
254 
257 {
258  return MatTMatMul_base(&MatrixHandler::IncCoef, out, in);
259 }
260 
263 {
264  return MatMatMul_base(&MatrixHandler::DecCoef, out, in);
265 }
266 
269 {
270  return MatTMatMul_base(&MatrixHandler::DecCoef, out, in);
271 }
272 
273 /* Matrix Vector product */
276  void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
277  VectorHandler& out, const VectorHandler& in) const
278 {
279  integer nr = iGetNumRows();
280  integer nc = iGetNumCols();
281 
282  if (out.iGetSize() != nr || in.iGetSize() != nc) {
283  silent_cerr("MatrixHandler::MatVecMul_base(): size mismatch "
284  "out(" << out.iGetSize() << ", 1) "
285  "= this(" << nr << ", " << nc << ") "
286  "* in(" << in.iGetSize() << ", 1)" << std::endl);
288  }
289 
290  for (integer r = 1; r <= nr; r++) {
291  doublereal d = 0.;
292 
293  for (integer c = 1; c <= nc; c++) {
294  d += this->operator()(r, c)*in(c);
295  }
296  (out.*op)(r, d);
297  }
298 
299  return out;
300 
301 }
302 
305  void (VectorHandler::*op)(integer iRow, const doublereal& dCoef),
306  VectorHandler& out, const VectorHandler& in) const
307 {
308  integer nr = iGetNumRows();
309  integer nc = iGetNumCols();
310 
311  if (out.iGetSize() != nc || in.iGetSize() != nr) {
312  silent_cerr("MatrixHandler::MatVecMul_base(): size mismatch "
313  "out(" << out.iGetSize() << ", 1) "
314  "= this(" << nr << ", " << nc << ")^T "
315  "* in(" << in.iGetSize() << ", 1)" << std::endl);
317  }
318 
319  for (integer r = 1; r <= nc; r++) {
320  doublereal d = 0.;
321 
322  for (integer c = 1; c <= nr; c++) {
323  d += this->operator()(c, r)*in(c);
324  }
325  (out.*op)(r, d);
326  }
327 
328  return out;
329 }
330 
333 {
334  /* Put is implemented resetting out first, then passing IncCoef()
335  * so that out-of-order assignments work */
336  out.Reset();
337  return MatVecMul_base(&VectorHandler::IncCoef, out, in);
338 }
339 
342 {
343  /* Put is implemented resetting out first, then passing IncCoef()
344  * so that out-of-order assignments work */
345  out.Reset();
346  return MatTVecMul_base(&VectorHandler::IncCoef, out, in);
347 }
348 
351 {
352  return MatVecMul_base(&VectorHandler::IncCoef, out, in);
353 }
354 
357 {
358  return MatTVecMul_base(&VectorHandler::IncCoef, out, in);
359 }
360 
363 {
364  return MatVecMul_base(&VectorHandler::DecCoef, out, in);
365 }
366 
369 {
370  return MatTVecMul_base(&VectorHandler::DecCoef, out, in);
371 }
372 
373 void
375  operator()(ix, iy) += inc;
376 }
377 
378 void
380  operator()(ix, iy) -= inc;
381 }
382 
383 void
385  operator()(ix, iy) = val;
386 }
387 
388 const doublereal&
390  return operator()(ix, iy);
391 }
392 
393 #define HAVE_CONDITION_NUMBER ((defined(HAVE_DGETRF_) || defined(HAVE_DGETRF)) && (defined(HAVE_DGECON_) || defined(HAVE_DGECON)))
394 
395 namespace {
396  void LapackCopyMatrix(const MatrixHandler& MH, std::vector<doublereal>& A, integer& M, integer& N)
397  {
398  M = MH.iGetNumRows();
399  N = MH.iGetNumCols();
400  A.resize(M*N);
401 
402  for (int i = 0; i < M; ++i) {
403  for (int j = 0; j < N; ++j) {
404  A[j * M + i] = MH(i + 1, j + 1);
405  }
406  }
407  }
408 
409  doublereal LapackMatrixNorm(const std::vector<doublereal>& A, const integer M, const integer N, enum MatrixHandler::Norm_t eNorm)
410  {
411  doublereal norm = 0.;
412 
413  switch (eNorm) {
415  for (int j = 0; j < N; ++j) {
416  doublereal csum = 0.;
417 
418  for (int i = 0; i < M; ++i) {
419  csum += std::abs(A[j * M + i]);
420  }
421 
422  if (csum > norm)
423  norm = csum;
424  }
425  break;
426 
428  for (int i = 0; i < N; ++i) {
429  doublereal rsum = 0.;
430 
431  for (int j = 0; j < M; ++j) {
432  rsum += std::abs(A[j * M + i]);
433  }
434 
435  if (rsum > norm)
436  norm = rsum;
437  }
438  break;
439 
440  default:
441  ASSERT(0);
443  }
444 
445  return norm;
446  }
447 }
448 
449 //#define DEBUG
450 
452 {
453 #if HAVE_CONDITION_NUMBER
454  integer M;
455  integer N;
456  std::vector<doublereal> A;
457 
458  LapackCopyMatrix(*this, A, M, N);
459 
460  const doublereal ANORM = LapackMatrixNorm(A, M, N, eNorm);
461 
462 #ifdef DEBUG
463  std::cerr << "ANORM=" << ANORM << std::endl;
464  std::cerr << "A=" << std::endl;
465  for (int i = 0; i < M; ++i) {
466  for (int j = 0; j < N; ++j) {
467  std::cerr << A[j * M + i] << '\t';
468  }
469  std::cerr << std::endl;
470  }
471 #endif // DEBUG
472 
473  integer INFO = 0;
474  std::vector<integer> IPIV(std::min(M,N));
475 
476  __FC_DECL__(dgetrf)(&M, &N, &A[0], &M, &IPIV[0], &INFO);
477 
478  ASSERT(INFO == 0); // should not fail because the Jacobian has already been factorised before
479 
480  std::vector<doublereal> WORK(4*N);
481  std::vector<integer> IWORK(N);
482  doublereal RCOND = 0.;
483  char norm;
484 
485  switch (eNorm) {
486  case NORM_1:
487  norm = '1';
488  break;
489  case NORM_INF:
490  norm = 'I';
491  break;
492  default:
493  ASSERT(0);
495  }
496 
497  __FC_DECL__(dgecon)(&norm, &N, &A[0], &M, &ANORM, &RCOND, &WORK[0], &IWORK[0], &INFO);
498 
499  ASSERT(INFO == 0); // should not fail
500 
501 #ifdef DEBUG
502  std::cerr << "RCOND=" << RCOND << std::endl;
503 #endif // DEBUG
504 
505  return 1. / RCOND;
506 #else // ! HAVE_CONDITION_NUMBER
507  silent_cerr("Condition number is not available in this version of MBDyn" << std::endl);
509 #endif // ! HAVE_CONDITION_NUMBER
510 }
511 
513 {
514  integer M;
515  integer N;
516  std::vector<doublereal> A;
517 
518  LapackCopyMatrix(*this, A, M, N);
519 
520  return LapackMatrixNorm(A, M, N, eNorm);
521 }
522 
523 std::ostream&
524 operator << (std::ostream& out, const MatrixHandler& MH)
525 {
526  integer nr = MH.iGetNumRows();
527  integer nc = MH.iGetNumCols();
528 
529  for (integer i = 1; i <= nr; i++) {
530  for (integer j = 1; j <= nc; j++) {
531  out << std::setw(16) << MH(i, j) << ' ';
532  }
533  out << std::endl;
534  }
535 
536  return out;
537 }
538 
539 /* MatrixHandler - end */
540 
virtual void Reset(void)=0
virtual MatrixHandler & MatMatIncMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:250
virtual integer iGetNumCols(void) const =0
virtual const doublereal & dGetCoef(integer iRow, integer iCol) const
Definition: mh.cc:389
virtual void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: mh.cc:374
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
virtual MatrixHandler & MatMatDecMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:262
virtual VectorHandler & MatVecMul_base(void(VectorHandler::*op)(integer iRow, const doublereal &dCoef), VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:275
virtual integer PacMat(void)
Definition: mh.cc:65
virtual VectorHandler & MatVecIncMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:350
std::ostream & operator<<(std::ostream &out, const MatrixHandler &MH)
Definition: mh.cc:524
virtual MatrixHandler & AddTo(MatrixHandler &MH) const =0
virtual VectorHandler & MatTVecDecMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:368
virtual MatrixHandler & operator-=(const SubMatrixHandler &SubMH)
Definition: mh.cc:99
virtual void IncCoef(integer iRow, const doublereal &dCoef)=0
virtual void DecCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: mh.cc:379
virtual VectorHandler & MatTVecIncMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:356
#define NO_OP
Definition: myassert.h:74
virtual doublereal Norm(enum Norm_t eNorm=NORM_1) const
Definition: mh.cc:512
virtual integer iGetSize(void) const =0
virtual VectorHandler & MatVecDecMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:362
virtual doublereal ConditionNumber(enum Norm_t eNorm=NORM_1) const
Definition: mh.cc:451
virtual void Reset(void)=0
virtual void DecCoef(integer iRow, const doublereal &dCoef)=0
virtual MatrixHandler & MatTMatMul_base(void(MatrixHandler::*op)(integer iRow, integer iCol, const doublereal &dCoef), MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:183
virtual MatrixHandler & SubFrom(MatrixHandler &MH) const =0
virtual MatrixHandler & operator+=(const SubMatrixHandler &SubMH)
Definition: mh.cc:92
virtual const doublereal & operator()(integer iRow, integer iCol) const =0
MatrixHandler & AddTo(MatrixHandler &MH) const
Definition: submat.h:1261
virtual ~MatrixHandler(void)
Definition: mh.cc:49
virtual void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: mh.cc:384
#define ASSERT(expression)
Definition: colamd.c:977
virtual VectorHandler & MatTVecMul_base(void(VectorHandler::*op)(integer iRow, const doublereal &dCoef), VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:304
virtual MatrixHandler & MatMatMul_base(void(MatrixHandler::*op)(integer iRow, integer iCol, const doublereal &dCoef), MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:135
virtual void ResizeReset(integer, integer)
Definition: mh.cc:56
MatrixHandler & SubFrom(MatrixHandler &MH) const
Definition: submat.h:1325
virtual MatrixHandler & MatTMatIncMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:256
static std::stack< cleanup * > c
Definition: cleanup.cc:59
virtual void Resize(integer, integer)=0
virtual MatrixHandler & MatTMatDecMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:268
virtual VectorHandler & MatTVecMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:341
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
virtual MatrixHandler & MatMatMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:232
virtual MatrixHandler & MatTMatMul(MatrixHandler &out, const MatrixHandler &in) const
Definition: mh.cc:241
virtual VectorHandler & MatVecMul(VectorHandler &out, const VectorHandler &in) const
Definition: mh.cc:332
virtual MatrixHandler & operator=(const MatrixHandler &MH)
Definition: mh.cc:72
virtual integer iGetNumRows(void) const =0
virtual MatrixHandler & ScalarMul(const doublereal &d)
Definition: mh.cc:119