MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
gpc.cc File Reference
#include "mbconfig.h"
#include <cmath>
#include "myassert.h"
#include "mynewmem.h"
#include "gpc.h"
Include dependency graph for gpc.cc:

Go to the source code of this file.

Functions

static int gpc_addmul (integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndim3, integer nrow3, integer ncol3, const doublereal *m3, integer ndimd, integer nrowd, integer ncold, doublereal *d)
 
static int gpc_mul (integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndimd, integer nrowd, integer ncold, doublereal *d)
 
static int gpc_add_mul (integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndim3, integer nrow3, integer ncol3, const doublereal *m3, integer ndimd, integer nrowd, integer ncold, doublereal *d)
 
static int gpc_addmul (integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer ncol2, const doublereal *m2, integer ndim3, const doublereal *m3, integer ndimd, doublereal *d)
 
static int gpc_mcopy (integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
 
static int gpc_mcopy_t (integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
 
static int gpc_zero (integer size, doublereal *v)
 
static int gpc_set (integer size, doublereal *v, const doublereal d)
 
static int gpc_build_matrices (integer ndimA, doublereal *A, integer ndimB, doublereal *B, integer ndimP, doublereal *P, integer ndimC, doublereal *C, integer nout, integer nin, integer s, integer pa, integer pb, doublereal *Theta)
 
int dgesvd (char *jobu, char *jobvt, integer *m, integer *n, doublereal *a, integer *lda, doublereal *s, doublereal *u, integer *ldu, doublereal *vt, integer *ldvt, doublereal *work, integer *lwork, integer *info)
 
static integer gpc_pinv (integer lda, integer m, integer n, doublereal *a, doublereal *s, integer ldu, doublereal *u, integer ldvt, doublereal *vt, integer lwork, doublereal *work)
 

Function Documentation

int dgesvd ( char *  jobu,
char *  jobvt,
integer m,
integer n,
doublereal a,
integer lda,
doublereal s,
doublereal u,
integer ldu,
doublereal vt,
integer ldvt,
doublereal work,
integer lwork,
integer info 
)

Referenced by gpc_pinv().

static int gpc_add_mul ( integer  ndim1,
integer  nrow1,
integer  ncol1,
const doublereal m1,
integer  ndim2,
integer  nrow2,
integer  ncol2,
const doublereal m2,
integer  ndim3,
integer  nrow3,
integer  ncol3,
const doublereal m3,
integer  ndimd,
integer  nrowd,
integer  ncold,
doublereal d 
)
inlinestatic

Definition at line 145 of file gpc.cc.

References gpc_addmul(), and gpc_mul().

149 {
150  if (m3) {
151  return gpc_addmul(ndim1, nrow1, ncol1, m1,
152  ndim2, nrow2, ncol2, m2,
153  ndim3, nrow3, ncol3, m3,
154  ndimd, nrowd, ncold, d);
155  } /* else */
156  return gpc_mul(ndim1, nrow1, ncol1, m1,
157  ndim2, nrow2, ncol2, m2,
158  ndimd, nrowd, ncold, d);
159 }
static int gpc_addmul(integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndim3, integer nrow3, integer ncol3, const doublereal *m3, integer ndimd, integer nrowd, integer ncold, doublereal *d)
Definition: gpc.cc:49
static int gpc_mul(integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndimd, integer nrowd, integer ncold, doublereal *d)
Definition: gpc.cc:101

Here is the call graph for this function:

static int gpc_addmul ( integer  ndim1,
integer  nrow1,
integer  ncol1,
const doublereal m1,
integer  ndim2,
integer  nrow2,
integer  ncol2,
const doublereal m2,
integer  ndim3,
integer  nrow3,
integer  ncol3,
const doublereal m3,
integer  ndimd,
integer  nrowd,
integer  ncold,
doublereal d 
)
inlinestatic

Definition at line 49 of file gpc.cc.

References ASSERT, and c.

Referenced by gpc_add_mul(), and gpc_build_matrices().

53 {
54  ASSERT(m1 != NULL);
55  ASSERT(m2 != NULL);
56  ASSERT(m3 != NULL);
57  ASSERT(d != NULL);
58 
59  ASSERT(nrow2 == ncol1);
60  ASSERT(nrow3 == nrow1);
61  ASSERT(ncol3 == ncol2);
62  ASSERT(nrowd == nrow1);
63  ASSERT(ncold == ncol2);
64 
65  ASSERT(ndim1 >= nrow1);
66  ASSERT(ndim2 >= ncol1);
67  ASSERT(ndim3 >= nrow1);
68  ASSERT(ndimd >= nrow1);
69 
70  doublereal* mm1 = (doublereal*)m1;
71  doublereal* mm2 = (doublereal*)m2+ncol2*ndim2;
72  doublereal* mm3 = (doublereal*)m3+ncol3*ndim3;
73  doublereal* dd = d+ncold*ndimd;
74 
75  for (integer c = ncold; c-- > 0; ) {
76  dd -= ndimd-nrowd;
77  mm2 -= ndim2;
78  mm3 -= ndim3-nrow3;
79  mm1 += nrow1;
80 
81  for (integer r = nrowd; r-- > 0; ) {
82  dd--;
83  mm1--;
84  mm3--;
85 
86  dd[0] = mm3[0];
87  for (integer k = ncol1; k-- > 0; ) {
88  dd[0] += mm1[k*ndim1]*mm2[k];
89  }
90  }
91  }
92 
93  return 0;
94 }
#define ASSERT(expression)
Definition: colamd.c:977
static std::stack< cleanup * > c
Definition: cleanup.cc:59
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static int gpc_addmul ( integer  ndim1,
integer  nrow1,
integer  ncol1,
const doublereal m1,
integer  ndim2,
integer  ncol2,
const doublereal m2,
integer  ndim3,
const doublereal m3,
integer  ndimd,
doublereal d 
)
inlinestatic

Definition at line 166 of file gpc.cc.

References ASSERT, and c.

170 {
171  ASSERT(m1 != NULL);
172  ASSERT(m2 != NULL);
173  ASSERT(m3 != NULL);
174  ASSERT(d != NULL);
175 
176  ASSERT(ndim1 >= nrow1);
177  ASSERT(ndim2 >= ncol1);
178  ASSERT(ndim3 >= nrow1);
179  ASSERT(ndimd >= nrow1);
180 
181  doublereal* dd = d+ncol2*ndimd;
182  doublereal* mm1 = (doublereal*)m1;
183  doublereal* mm2 = (doublereal*)m2+ncol2*ndim2;
184  doublereal* mm3 = (doublereal*)m3+ncol2*ndim3;
185 
186  for (integer c = ncol2; c-- > 0; ) {
187  dd -= ndimd-nrow1;
188  mm2 -= ndim2;
189  mm3 -= ndim3-nrow1;
190  mm1 += nrow1;
191 
192  for (integer r = nrow1; r-- > 0; ) {
193  dd--;
194  mm1--;
195  mm3--;
196 
197  dd[0] = mm3[0];
198  for (integer k = ncol1; k-- > 0; ) {
199  dd[0] += mm1[k*ndim1]*mm2[k];
200  }
201  }
202  }
203 
204  return 0;
205 }
#define ASSERT(expression)
Definition: colamd.c:977
static std::stack< cleanup * > c
Definition: cleanup.cc:59
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static int gpc_build_matrices ( integer  ndimA,
doublereal A,
integer  ndimB,
doublereal B,
integer  ndimP,
doublereal P,
integer  ndimC,
doublereal C,
integer  nout,
integer  nin,
integer  s,
integer  pa,
integer  pb,
doublereal Theta 
)
inlinestatic

Definition at line 284 of file gpc.cc.

References gpc_addmul(), gpc_mcopy(), gpc_mcopy_t(), gpc_mul(), and gpc_zero().

291 {
292  /*
293  * matrici: A, B, P (, C se definita)
294  * dimensioni: dimA, dimB, dimP (, dimC se definita) = s*nout
295  * ncolA = pa*nout
296  * ncolB = pb*nin
297  * ncolP = s*nin
298  * (ncolC = pa*nout se definita)
299  */
300 
301  /*
302  * inizializzo le matrici (attenzione che theta
303  * e' organizzata per righe)
304  */
305  integer size = nout*pa+nin*(pb+1);
306 
307  if (C != NULL && pa > 0) {
308  size += nout*pa;
309  }
310 
311  if (pa > 0) {
312  /* inizializza A con a_1 .. a_pa */
313  doublereal* source = Theta;
314  doublereal* dest = A+nout*(s-1);
315  gpc_mcopy_t(size, nout, nout*pa, source, ndimA, dest);
316 
317  if (C != NULL) {
318  /* inizializza c con c_1 .. c_pa */
319  source = Theta+nout*pa+nin*(pb+1);
320  dest = C+nout*(s-1);
321  gpc_mcopy_t(size, nout, nin*(pb+1),
322  source, ndimB, dest);
323  }
324  }
325 
326  /* inizializza B con b_1 .. b_pb */
327  /* FIXME: move declarations at top to make this C */
328  doublereal* source = Theta+nout*pa+nin;
329  doublereal* dest = B+nout*(s-1);
330  gpc_mcopy_t(size, nout, nin*pb, source, ndimB, dest);
331 
332  /* inizializza P con b_0 */
333  gpc_zero(ndimP*(nin*s), P);
334  source = Theta+nout*pa;
335  dest = P+nout*(s-1)+ndimP*(nin*(s-1));
336  gpc_mcopy_t(size, nout, nin, source, ndimP, dest);
337 
338  /*
339  * le matrici si assumono gia' inizializzate;
340  * significa che all'ultimo blocco-riga contengono:
341  * A le matrici a_1 .. a_pa,
342  * B le matrici b_1 .. b_pb,
343  * P zeri terminati dalla matrice b_0,
344  * ( C le matrici c_1 .. c_pa se definita)
345  */
346 
347  /* ciclo principale */
348  for (integer i = s-1; i > 0; i--) {
349  doublereal* m1 = A+nout*i; /* a_1^(l-1) */
350 
351  if (pa > 0) {
352  for (integer j = 1; j <= pa-1; j++) {
353  /* a_j^l = a_1^(l-1)*a_j^0+a_(j+1)^(l-1) */
354 
355  /* a_j^l */
356  doublereal* dest =
357  A+nout*(i-1)+ndimA*(nout*(j-1));
358 
359  /* a_j^0 */
360  doublereal* m2 =
361  A+nout*(s-1)+ndimA*(nout*(j-1));
362 
363  /* a_(j+1)^(l-1) */
364  doublereal* m3 = A+nout*i+ndimA*(nout*j);
365 
366  gpc_addmul(ndimA, nout, nout, m1,
367  ndimA, nout, nout, m2,
368  ndimA, nout, nout, m3,
369  ndimA, nout, nout, dest);
370  }
371 
372  /* a_pa^l = a_1^(l-1)*a_pa^0 */
373  /* FIXME: move declarations at top to make this C */
374 
375  /* a_pa^l */
376  doublereal* dest = A+nout*(i-1)+ndimA*(nout*(pa-1));
377 
378  /* a_pa^0 */
379  doublereal* m2 = A+nout*(s-1)+ndimA*(nout*(pa-1));
380 
381  gpc_mul(ndimA, nout, nout, m1,
382  ndimA, nout, nout, m2,
383  ndimA, nout, nout, dest);
384  }
385 
386  if (pb > 0) {
387  for (integer j = 1; j <= pb-1; j++) {
388  /* b_j^l = a_1^(l-1)*b_j^0+b_(j+1)^(l-1) */
389 
390  /* b_j^l */
391  doublereal* dest =
392  B+nout*(i-1)+ndimB*(nin*(j-1));
393 
394  /* b_j^0 */
395  doublereal* m2 =
396  B+nout*(s-1)+ndimB*(nin*(j-1));
397 
398  /* b_(j+1)^(l-1) */
399  doublereal* m3 = B+nout*i+ndimB*(nin*j);
400 
401  gpc_addmul(ndimA, nout, nout, m1,
402  ndimB, nout, nin, m2,
403  ndimB, nout, nin, m3,
404  ndimB, nout, nin, dest);
405  }
406 
407  /* b_pb^l = a_1^(l-1)*b_pb^0 */
408  /* FIXME: move declarations at top to make this C */
409 
410  /* b_pb^l */
411  doublereal* dest = B+nout*(i-1)+ndimB*(nin*(pb-1));
412 
413  /* b_pb^0 */
414  doublereal* m2 = B+nout*(s-1)+ndimB*(nin*(pb-1));
415 
416  gpc_mul(ndimA, nout, nout, m1,
417  ndimB, nout, nin, m2,
418  ndimB, nout, nin, dest);
419  }
420 
421  if (C != NULL && pa > 0) {
422  for (integer j = 1; j <= pa-1; j++) {
423  /* c_j^l = a_1^(l-1)*c_j^0+c_(j+1)^(l-1) */
424 
425  /* c_j^l */
426  doublereal* dest =
427  C+nout*(i-1)+ndimC*(nout*(j-1));
428 
429  /* c_j^0 */
430  doublereal* m2 =
431  C+nout*(s-1)+ndimC*(nout*(j-1));
432 
433  /* c_(j+1)^(l-1) */
434  doublereal* m3 = C+nout*i+ndimC*(nout*j);
435 
436  gpc_addmul(ndimA, nout, nout, m1,
437  ndimC, nout, nout, m2,
438  ndimC, nout, nout, m3,
439  ndimC, nout, nout, dest);
440  }
441 
442  /* c_pa^l = a_1^(l-1)*c_pa^0 */
443  /* FIXME: move declarations at top to make this C */
444 
445  /* c_pa^l */
446  doublereal* dest = C+nout*(i-1)+ndimC*(nout*(pa-1));
447 
448  /* c_pa^0 */
449  doublereal* m2 = C+nout*(s-1)+ndimC*(nout*(pa-1));
450 
451  gpc_mul(ndimA, nout, nout, m1,
452  ndimC, nout, nout, m2,
453  ndimC, nout, nout, dest);
454  }
455 
456  do {
457  do {
458  /* p_l^l .. p_(s-1)^l =
459  * p_(l+1)^(l-1) .. p_s^(l-1) */
460 
461  /* p_(l+1)^(l-1) .. p_s^(l-1) */
462  doublereal* source = P+nout*i+ndimP*(nin*i);
463 
464  /* p_l^l .. p_(s-1)^l */
465  doublereal* dest =
466  P+nout*(i-1)+ndimP*(nin*(i-1));
467 
468  gpc_mcopy(ndimP, nout, nin*(s-i), source,
469  ndimP, dest);
470  } while (0);
471 
472  /* p_s^l = a_1^(l-1)*p_s^0+b_1^(l-1) */
473  /* FIXME: move declarations at top to make this C */
474 
475  /* p_s^l */
476  doublereal* dest = P+nout*(i-1)+ndimP*(nin*(s-1));
477 
478  /* p_s^0 */
479  doublereal* m2 = P+nout*(s-1)+ndimP*(nin*(s-1));
480 
481  /* b_1^(l-1) */
482  doublereal* m3 = B+nout*i;
483 
484  gpc_addmul(ndimA, nout, nout, m1,
485  ndimP, nout, nin, m2,
486  ndimB, nout, nin, m3,
487  ndimP, nout, nin, dest);
488  } while (0);
489  }
490 
491  return 0;
492 }
static int gpc_zero(integer size, doublereal *v)
Definition: gpc.cc:258
static int gpc_mcopy(integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
Definition: gpc.cc:212
static int gpc_addmul(integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndim3, integer nrow3, integer ncol3, const doublereal *m3, integer ndimd, integer nrowd, integer ncold, doublereal *d)
Definition: gpc.cc:49
static int gpc_mul(integer ndim1, integer nrow1, integer ncol1, const doublereal *m1, integer ndim2, integer nrow2, integer ncol2, const doublereal *m2, integer ndimd, integer nrowd, integer ncold, doublereal *d)
Definition: gpc.cc:101
static int gpc_mcopy_t(integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
Definition: gpc.cc:235
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

static int gpc_mcopy ( integer  ndims,
integer  nrows,
integer  ncols,
const doublereal s,
integer  ndimd,
doublereal d 
)
inlinestatic

Definition at line 212 of file gpc.cc.

Referenced by gpc_build_matrices().

214 {
215  doublereal* ss = (doublereal*)s+ndims*ncols;
216  doublereal* dd = d+ndimd*ncols;
217 
218  for (integer j = ncols; j-- > 0; ) {
219  ss -= ndims;
220  dd -= ndimd;
221 
222  for (integer i = nrows; i-- > 0; ) {
223  dd[i] = ss[i];
224  }
225  }
226 
227  return 0;
228 }
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static int gpc_mcopy_t ( integer  ndims,
integer  nrows,
integer  ncols,
const doublereal s,
integer  ndimd,
doublereal d 
)
inlinestatic

Definition at line 235 of file gpc.cc.

Referenced by gpc_build_matrices().

237 {
238  doublereal* ss = (doublereal*)s+ncols;
239  doublereal* dd = d+ndimd*ncols;
240 
241  for (integer j = ncols; j-- > 0; ) {
242  dd -= ndimd;
243  ss--;
244 
245  for (integer i = nrows; i-- > 0; ) {
246  dd[i] = ss[ndims*i];
247  }
248  }
249 
250  return 0;
251 }
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static int gpc_mul ( integer  ndim1,
integer  nrow1,
integer  ncol1,
const doublereal m1,
integer  ndim2,
integer  nrow2,
integer  ncol2,
const doublereal m2,
integer  ndimd,
integer  nrowd,
integer  ncold,
doublereal d 
)
inlinestatic

Definition at line 101 of file gpc.cc.

References ASSERT, and c.

Referenced by gpc_add_mul(), and gpc_build_matrices().

104 {
105  ASSERT(m1 != NULL);
106  ASSERT(m2 != NULL);
107  ASSERT(d != NULL);
108 
109  ASSERT(nrow2 == ncol1);
110  ASSERT(nrowd == nrow1);
111  ASSERT(ncold == ncol2);
112 
113  ASSERT(ndim1 >= nrow1);
114  ASSERT(ndim2 >= ncol1);
115  ASSERT(ndimd >= nrow1);
116 
117  doublereal* mm1 = (doublereal*)m1;
118  doublereal* mm2 = (doublereal*)m2+ncol2*ndim2;
119  doublereal* dd = d+ncold*ndimd;
120 
121  for (integer c = ncold; c-- > 0; ) {
122  dd -= ndimd-nrowd;
123  mm2 -= ndim2;
124  mm1 += nrow1;
125 
126  for (integer r = nrowd; r-- > 0; ) {
127  dd--;
128  mm1--;
129 
130  dd[0] = 0.;
131  for (integer k = ncol1; k-- > 0; ) {
132  dd[0] += mm1[k*ndim1]*mm2[k];
133  }
134  }
135  }
136 
137  return 0;
138 }
#define ASSERT(expression)
Definition: colamd.c:977
static std::stack< cleanup * > c
Definition: cleanup.cc:59
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static integer gpc_pinv ( integer  lda,
integer  m,
integer  n,
doublereal a,
doublereal s,
integer  ldu,
doublereal u,
integer  ldvt,
doublereal vt,
integer  lwork,
doublereal work 
)
inlinestatic

Definition at line 518 of file gpc.cc.

References a, dgesvd(), and grad::fabs().

Referenced by GPC_LAPACK_pinv::Inv().

523 {
524 #ifdef USE_LAPACK
525  /*
526  * esegue la SVD di a, m x n, con routines di LAPACK;
527  * ritorna la pseudo-inversa in a, con dimensioni invertite: n x m
528  * la matrice pinv(a) viene organizzata per righe
529  */
530  integer info = 0;
531  static char jobu = 'S';
532  static char jobvt = 'S';
533 
534  __FC_DECL__(dgesvd)(&jobu, &jobvt, &m, &n, a, &lda, s,
535  u, &ldu, vt, &ldvt, work, &lwork, &info);
536 #else /* !USE_LAPACK */
537  integer info = 1;
538  silent_cerr("warning: LAPACK libraries are not available," << std::endl
539  << "so the pseudo-inversion will not be performed" << std::endl);
540 #endif /* !USE_LAPACK */
541 
542  if (info != 0) {
543  return info;
544  }
545 
546  /* corregge con gli inversi dei SV */
547  /* FIXME: mettere qualcosa tipo RCOND */
548  const doublereal THR = s[0]*1.e-12;
549  integer x = std::min(m, n);
550  for (integer i = x; i-- > 0; ) {
551  doublereal d = s[i];
552  if (fabs(d) < THR) {
553  d = 0.;
554  } else {
555  d = 1./d;
556  }
557 
558  doublereal* p = u+i*ldu;
559  for (integer k = m; k-- > 0; ) {
560  p[k] *= d;
561  }
562  }
563 
564  /* mette l'inversa in a (organizzata per righe) */
565  for (integer i = n; i-- > 0; ) {
566  doublereal* aa = a+m*i;
567  doublereal* vvt = vt+i*ldvt;
568  for (integer j = m; j-- > 0; ) {
569  doublereal* uu = u+j;
570  aa[j] = 0.;
571  for (integer k = x; k-- > 0; ) {
572  aa[j] += vvt[k]*uu[k*ldu];
573  }
574  }
575  }
576 
577  return info;
578 }
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
int dgesvd(char *jobu, char *jobvt, integer *m, integer *n, doublereal *a, integer *lda, doublereal *s, doublereal *u, integer *ldu, doublereal *vt, integer *ldvt, doublereal *work, integer *lwork, integer *info)
static const doublereal a
Definition: hfluid_.h:289
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51

Here is the call graph for this function:

static int gpc_set ( integer  size,
doublereal v,
const doublereal  d 
)
inlinestatic

Definition at line 271 of file gpc.cc.

272 {
273  for (integer i = size; i-- > 0; ) {
274  v[i] = d;
275  }
276  return 0;
277 }
long int integer
Definition: colamd.c:51
static int gpc_zero ( integer  size,
doublereal v 
)
inlinestatic

Definition at line 258 of file gpc.cc.

Referenced by gpc_build_matrices().

259 {
260  for (integer i = size; i-- > 0; ) {
261  v[i] = 0.;
262  }
263  return 0;
264 }
long int integer
Definition: colamd.c:51