MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
gpc.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/elec/gpc.cc,v 1.32 2017/01/12 14:46:22 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 /* funzioni di base per le operazioni con le matrici */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <cmath>
37 
38 #include "myassert.h"
39 #include "mynewmem.h"
40 #include "gpc.h"
41 
42 /* gpc utility functions - begin */
43 
44 /*
45  * esegue d = m1*m2+m3
46  */
47 
48 static inline int
49 gpc_addmul(integer ndim1, integer nrow1, integer ncol1, const doublereal* m1,
50  integer ndim2, integer nrow2, integer ncol2, const doublereal* m2,
51  integer ndim3, integer nrow3, integer ncol3, const doublereal* m3,
52  integer ndimd, integer nrowd, integer ncold, doublereal* d)
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 }
95 
96 /*
97  * esegue d = m1*m2
98  */
99 
100 static inline int
101 gpc_mul(integer ndim1, integer nrow1, integer ncol1, const doublereal* m1,
102  integer ndim2, integer nrow2, integer ncol2, const doublereal* m2,
103  integer ndimd, integer nrowd, integer ncold, doublereal* d)
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 }
139 
140 /*
141  * esegue d = m1*m2; se m3 e' definita, d = m1*m2+m3
142  */
143 
144 static inline int
145 gpc_add_mul(integer ndim1, integer nrow1, integer ncol1, const doublereal* m1,
146  integer ndim2, integer nrow2, integer ncol2, const doublereal* m2,
147  integer ndim3, integer nrow3, integer ncol3, const doublereal* m3,
148  integer ndimd, integer nrowd, integer ncold, doublereal* d)
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 }
160 
161 /*
162  * esegue d = m1*m2+m3 senza check sulle dimensioni
163  */
164 
165 static inline int
166 gpc_addmul(integer ndim1, integer nrow1, integer ncol1, const doublereal* m1,
167  integer ndim2, integer ncol2, const doublereal* m2,
168  integer ndim3, const doublereal* m3,
169  integer ndimd, doublereal* d)
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 }
206 
207 /*
208  * copia una matrice su un'altra
209  */
210 
211 static inline int
212 gpc_mcopy(integer ndims, integer nrows, integer ncols, const doublereal* s,
213  integer ndimd, doublereal* d)
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 }
229 
230 /*
231  * copia una matrice orientata per righe su una orientata per colonne
232  */
233 
234 static inline int
235 gpc_mcopy_t(integer ndims, integer nrows, integer ncols, const doublereal* s,
236  integer ndimd, doublereal* d)
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 }
252 
253 /*
254  * azzera un vettore
255  */
256 
257 static inline int
258 gpc_zero(integer size, doublereal* v)
259 {
260  for (integer i = size; i-- > 0; ) {
261  v[i] = 0.;
262  }
263  return 0;
264 }
265 
266 /*
267  * setta un vettore
268  */
269 
270 static inline int
271 gpc_set(integer size, doublereal* v, const doublereal d)
272 {
273  for (integer i = size; i-- > 0; ) {
274  v[i] = d;
275  }
276  return 0;
277 }
278 
279 /*
280  * costruisce le matrici grezze di predizione
281  */
282 
283 static inline int
284 gpc_build_matrices(integer ndimA, doublereal* A,
285  integer ndimB, doublereal* B,
286  integer ndimP, doublereal* P,
287  integer ndimC, doublereal* C,
288  integer nout, integer nin,
289  integer s, integer pa, integer pb,
290  doublereal* Theta)
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 }
493 
494 #ifdef __cplusplus
495 extern "C" {
496 #endif /* __cplusplus */
497 
498 extern int
499 __FC_DECL__(dgesvd)(char* jobu, char* jobvt,
500  integer* m, integer* n,
501  doublereal* a, integer* lda,
502  doublereal* s,
503  doublereal* u, integer* ldu,
504  doublereal* vt, integer* ldvt,
505  doublereal* work, integer* lwork,
506  integer* info);
507 
508 #ifdef __cplusplus
509 }
510 #endif /* __cplusplus */
511 
512 /*
513  * esegue la pseudoinversa di una matrice,
514  * sovrascrivendola orientata per righe
515  */
516 
517 static inline integer
518 gpc_pinv(integer lda, integer m, integer n, doublereal* a,
519  doublereal* s,
520  integer ldu, doublereal* u,
521  integer ldvt, doublereal* vt,
522  integer lwork, doublereal* work)
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 }
579 
580 /* GPCInv - begin */
581 
582 GPCInv::GPCInv(void)
583 : pdBase(NULL)
584 {
585  NO_OP;
586 }
587 
588 GPCInv::~GPCInv(void)
589 {
590  if (pdBase != NULL) {
591  SAFEDELETEARR(pdBase);
592  }
593 }
594 
595 /* GPCInv - end */
596 
597 
598 /* GPC_LAPACK_pinv - begin */
599 
600 GPC_LAPACK_pinv::GPC_LAPACK_pinv(integer m, integer n)
601 : GPCInv(),
602 m(m), n(n),
603 iMin(0),
604 iMax(0),
605 iWork(0),
606 pdS(NULL),
607 pdU(NULL),
608 pdVt(NULL),
609 pdWork(NULL)
610 {
611  ASSERT(m > 0);
612  ASSERT(n > 0);
613 
614  iMin = std::min(m, n); /* usati per vari parametri */
615  iMax = std::max(m, n); /* serve solo qui, non serve tenerlo */
616 
617  iWork = std::max(3*iMin+iMax, 5*iMin-4); /* aumentare per maggiore eff. */
618 
619  /* aree di lavoro per SVD:
620  * s: (min(m,n))
621  * u: (ldu*min(m,n))
622  * ldu >= m
623  * vt: (ldvt*n)
624  * ldvt >= min(m,n)
625  * work: lwork
626  * lwork >= max(3*min(m,n)+max(m,n),5*min(m,n)-4)
627  */
628 
629  integer i = iMin /* s */
630  +m*iMin /* u */
631  +iMin*n /* vt */
632 #ifndef OPTIMIZE_WORK
633  +iWork; /* work */
634 #else /* OPTIMIZE_WORK */
635  ;
636 
637  /* per ottimizzare le dimensioni di work: viene allocato a parte */
638  SAFENEWARR(pdWork, doublereal, iWork);
639 #endif /* OPTIMIZE_WORK */
640 
641  SAFENEWARR(pdBase, doublereal, i);
642 
643  pdS = pdBase;
644  pdU = pdS+iMin;
645  pdVt = pdU+m*iMin;
646 
647 #ifndef OPTIMIZE_WORK
648  pdWork = pdVt+iMin*n;
649 #endif /* !OPTIMIZE_WORK */
650 }
651 
652 GPC_LAPACK_pinv::~GPC_LAPACK_pinv(void)
653 {
654 #ifndef OPTIMIZE_WORK
655  NO_OP; /* pdBase viene distrutto da GPCInv */
656 #else /* OPTIMIZE_WORK */
657  SAFEDELETEARR(pdWork);
658 #endif /* OPTIMIZE_WORK */
659 }
660 
661 integer
662 GPC_LAPACK_pinv::Inv(integer ndima, integer nrowa,
663  integer ncola, doublereal* a)
664 {
665  ASSERT(nrowa == m);
666  ASSERT(ncola == n);
667 
668  integer info = gpc_pinv(ndima, nrowa, ncola, a,
669  pdS, m, pdU, iMin, pdVt, iWork, pdWork);
670 
671  if (info != 0) {
672  return info;
673  }
674 
675 #ifdef OPTIMIZE_WORK
676  if (((integer *)pdWork)[0] > iWork) {
677  iWork = ((integer *)pdWork)[0];
678  SAFEDELETEARR(pdWork);
679  SAFENEWARR(pdWork, doublereal, iWork);
680  }
681 #endif /* OPTIMIZE_WORK */
682 
683  return integer(0);
684 }
685 
686 /* GPC_LAPACK_pinv - end */
687 
688 
689 #ifdef USE_MESCHACH
690 
691 /* GPC_Meschach_QRinv - begin */
692 GPC_Meschach_QRinv::GPC_Meschach_QRinv(integer m, integer n)
693 : m(m),
694 n(n),
695 A(MNULL),
696 diag(VNULL),
697 x(VNULL),
698 b(VNULL),
699 pivot(PNULL)
700 {
701  ASSERT(m > 0);
702  ASSERT(n > 0);
703 
704  if ((A = m_get(m, n)) == MNULL) {
705  silent_cerr("A = m_get(" << m << ',' << n << ") failed" << std::endl);
706  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
707  }
708 
709  if ((diag = v_get(std::min(m, n))) == VNULL) {
710  silent_cerr("diag = v_get(" << std::min(m, n) << ") failed" << std::endl);
711  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
712  }
713 
714  if ((x = v_get(n)) == VNULL) {
715  silent_cerr("x = v_get(" << n << ") failed" << std::endl);
716  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
717  }
718 
719  if ((b = v_get(m)) == VNULL) {
720  silent_cerr("b = v_get(" << m << ") failed" << std::endl);
721  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
722  }
723 
724  if ((pivot = px_get(n)) == PNULL) {
725  silent_cerr("pivot = px_get(" << n << ") failed" << std::endl);
726  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
727  }
728 }
729 
730 GPC_Meschach_QRinv::~GPC_Meschach_QRinv(void)
731 {
732  PX_FREE(pivot);
733  V_FREE(b);
734  V_FREE(x);
735  V_FREE(diag);
736  M_FREE(A);
737 }
738 
739 integer
740 GPC_Meschach_QRinv::Inv(integer ndima, integer nrowa,
741  integer ncola, doublereal* a)
742 {
743  ASSERT(nrowa == m);
744  ASSERT(ncola == n);
745  ASSERT(ndima >= m);
746  ASSERT(a != NULL);
747 
748  /* copia a in A */
749  for (int j = n; j-- > 0; ) {
750  doublereal* p = a+ndima*j;
751 
752  for (int i = m; i-- > 0; ) {
753  A->me[i][j] = p[i];
754  }
755  }
756 
757  QRCPfactor(A, diag, pivot);
758 
759  /* puo' essere ottimizzata con accesso diretto alla fattorizzata */
760  for (int j = m; j-- > 0; ) {
761  v_zero(b);
762  b->ve[j] = 1.;
763  QRCPsolve(A, diag, pivot, b, x);
764  doublereal* p = a+j;
765 
766  for (int i = n; i-- > 0; ) {
767  p[i*m] = x->ve[i];
768  }
769  }
770 
771  return 0;
772 }
773 
774 /* GPC_Meschach_QRinv - end */
775 
776 #endif /* USE_MESCHACH */
777 
778 
779 /* GPCDesigner - begin */
780 
781 GPCDesigner::GPCDesigner(integer iNumOut, integer iNumIn,
782  integer iOrdA, integer iOrdB,
783  integer iPredS, integer iContrS,
784  integer iPredH, integer iContrH,
785  doublereal dPF)
786 : iNumOutputs(iNumOut),
787 iNumInputs(iNumIn),
788 iOrderA(iOrdA),
789 iOrderB(iOrdB),
790 iPredStep(iPredS),
791 iContrStep(iContrS),
792 iPredHor(iPredH),
793 iContrHor(iContrH),
794 dPeriodicFactor(dPF),
795 pdBase(NULL),
796 pdA(NULL),
797 pdB(NULL),
798 pdP(NULL),
799 pdC(NULL),
800 pdac(NULL),
801 pdbc(NULL),
802 pdmd(NULL),
803 pdcc(NULL)
804 {
805  ASSERT(iNumOutputs > 0);
806  ASSERT(iNumInputs > 0);
807  ASSERT(iPredStep > 0);
808  ASSERT(iContrStep > 0);
809 }
810 
811 GPCDesigner::~GPCDesigner(void)
812 {
813  SAFEDELETEARR(pdBase);
814 }
815 
816 
817 void
818 GPCDesigner::DesignControl(const doublereal* /* pdTheta */ ,
819  doublereal** ppda,
820  doublereal** ppdb,
821  doublereal** ppdm,
822  doublereal** ppdc)
823 {
824  if (ppda != NULL) {
825  *ppda = pdac;
826  }
827 
828  if (ppdb != NULL) {
829  *ppdb = pdbc;
830  }
831 
832  if (ppdm != NULL) {
833  *ppdm = pdmd;
834  }
835 
836  if (ppdc != NULL) {
837  *ppdc = pdcc;
838  }
839 }
840 
841 /* GPCDesigner - end */
842 
843 
844 /* DeadBeat - begin */
845 #ifdef USE_DBC
846 DeadBeat::DeadBeat(integer iNumOut, integer iNumIn,
847  integer iOrdA, integer iOrdB,
848  integer iPredS, integer iContrS,
849  doublereal dPF, flag f)
850 : GPCDesigner(iNumOut, iNumIn, iOrdA, iOrdB, iPredS, iContrS, iContrS, 0, dPF),
851 iDim(iNumOutputs*iPredStep),
852 iTmpRows(iNumOutputs*(iPredStep-iPredHor)),
853 iTmpCols(iNumInputs*(iContrStep-0)),
854 f_armax(f),
855 pInv(NULL)
856 {
857 #if !defined(USE_MESCHACH) && !defined(USE_LAPACK)
858 #error "need LAPACK or Meschach for pseudo-inversion"
859 #endif /* !USE_MESCHACH && !USE_LAPACK */
860  switch (1) { /* FIXME: mettere un flag */
861 #ifdef USE_MESCHACH
862  case 1:
863  SAFENEWWITHCONSTRUCTOR(pInv,
864  GPC_Meschach_QRinv,
865  GPC_Meschach_QRinv(iTmpRows, iTmpCols));
866  break;
867 #endif /* USE_MESCHACH */
868 #ifdef USE_LAPACK
869  case 2:
870  SAFENEWWITHCONSTRUCTOR(pInv,
871  GPC_LAPACK_pinv,
872  GPC_LAPACK_pinv(iTmpRows, iTmpCols));
873  break;
874 #endif /* USE_LAPACK */
875  }
876 
877  /* note that iPredHor == iContrStep */
878 
879  integer i = iDim*(iNumOutputs*iOrderA) /* A */
880  +iDim*(iNumInputs*iOrderB) /* B */
881  +iDim*(iNumInputs*iPredStep) /* P */
882  +iNumInputs*(iNumOutputs*iOrderA) /* ac */
883  +iNumInputs*(iNumInputs*iOrderB) /* bc */
884  +iNumInputs*iTmpRows; /* md */
885 
886  if (f_armax) {
887  i += iDim*(iNumOutputs*iOrderA) /* C */
888  +iNumInputs*(iNumOutputs*iOrderA); /* cc */
889  }
890 
891  SAFENEWARR(pdBase, doublereal, i);
892 
893  for (integer j = i; j-- > 0; ) {
894  pdBase[j] = 0.;
895  }
896 
897  pdA = pdBase;
898  pdB = pdA+iDim*(iNumOutputs*iOrderA);
899  pdP = pdB+iDim*(iNumInputs*iOrderB);
900  pdac = pdP+iDim*(iNumInputs*iPredStep);
901  pdbc = pdac+iNumInputs*(iNumOutputs*iOrderA);
902  pdmd = pdbc+iNumInputs*(iNumInputs*iOrderB);
903 
904  pdPTmp = pdP+iDim*(iNumInputs*(iPredStep-iContrStep));
905 
906  if (f_armax) {
907  pdC = pdmd+iNumInputs*iTmpRows;
908  pdcc = pdC+iDim*(iNumOutputs*iOrderA);
909  }
910 }
911 
912 DeadBeat::~DeadBeat(void)
913 {
914  NO_OP;
915 }
916 
917 void
918 DeadBeat::DesignControl(const doublereal* pdTheta,
919  doublereal** ppda,
920  doublereal** ppdb,
921  doublereal** ppdm,
922  doublereal** ppdc)
923 {
924  ASSERT(pdTheta != NULL);
925 
926  /*
927  * si assume che pdTheta contenga le stime delle matrici a_1 .. a_pa,
928  * b_0 .. b_pb disposte per righe, ovvero:
929  * a_1(1,1), a_1(1,2), ..., a_1(1,m), a_2(1,1), ...,
930  * a_pa(1,m),b_0(1,1),...
931  */
932 
933  gpc_build_matrices(iDim, pdA,
934  iDim, pdB,
935  iDim, pdP,
936  iDim, pdC, /* se f_armax == 0, pdC = NULL */
937  iNumOutputs, iNumInputs,
938  iPredStep, iOrderA, iOrderB,
939  (doublereal*)pdTheta);
940 
941  /* l'inversore sa come invertire, e mette l'inversa in pdPTmp */
942  integer info = pInv->Inv(iDim, iTmpRows, iTmpCols, pdPTmp);
943 
944  if (info < 0) {
945  silent_cerr("DeadBeat::DesignControl(): illegal value in "
946  << -info << "-th argument of dgesvd()" << std::endl);
947  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
948  } else if (info > 0) {
949  silent_cerr("DeadBeat::DesignControl(): error in dgesvd()" << std::endl);
950  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
951  } /* else: OK */
952 
953  /* recupero delle matrici (pinv(P) e' organizzata per righe) */
954 
955  /* copia le ultime righe dell'inversa di p, che sta in pdPTmp, in md */
956  doublereal* p = pdPTmp+iTmpRows*(iNumInputs*(iContrStep-1));
957  for (integer i = iTmpRows*iNumInputs; i-- > 0; ) {
958  pdmd[i] = p[i];
959  }
960 
961  /* anche ac, bc (e cc) vengono organizzate per righe */
962  doublereal cc = dPeriodicFactor*dPeriodicFactor;
963  for (integer i = iNumInputs; i-- > 0; ) {
964  doublereal* pm = pdmd+iTmpRows*i;
965 
966  /* ac = -md*A */
967  doublereal* p = pdac+i*iNumOutputs*iOrderA;
968  for (integer j = iNumOutputs*iOrderA; j-- > 0; ) {
969  doublereal* pa = pdA+iDim*j;
970 
971  p[j] = pm[j]*cc;
972  for (integer k = iTmpRows; k-- > 0; ) {
973  /* p[j] -= pdmd[iTmpRows*i+k]*pdA[k+iDim*j]; */
974  p[j] -= pm[k]*pa[k];
975  }
976  }
977 
978  /* bc = -md*B */
979  p = pdbc+i*iNumInputs*iOrderB;
980  for (integer j = iNumInputs*iOrderB; j-- > 0; ) {
981  doublereal* pb = pdB+iDim*j;
982  p[j] = 0.;
983  for (integer k = iTmpRows; k-- > 0; ) {
984  /* p[j] -= pdmd[iTmpRows*i+k]*pdB[k+iDim*j]; */
985  p[j] -= pm[k]*pb[k];
986  }
987  }
988 
989  if (f_armax) {
990  /* cc = -md*C */
991  p = pdcc+i*iNumOutputs*iOrderA;
992  for (integer j = iNumOutputs*iOrderA; j-- > 0; ) {
993  doublereal* pc = pdC+iDim*j;
994  p[j] = 0.;
995  for (integer k = iTmpRows; k-- > 0; ) {
996  /* p[j] -= pdmd[iTmpRows*i+k]
997  *pdC[k+iDim*j]; */
998  p[j] -= pm[k]*pc[k];
999  }
1000  }
1001  }
1002  }
1003 
1004  /* linka i puntatori alle matrici, se richiesto */
1005  GPCDesigner::DesignControl(NULL, ppda, ppdb, ppdm, ppdc);
1006 }
1007 #endif /* USE_DBC */
1008 
1009 /* DeadBeat - end */
1010 
1011 
1012 /* GPC - begin */
1013 
1014 #ifdef USE_DBC
1015 GPC::GPC(integer iNumOut, integer iNumIn,
1016  integer iOrdA, integer iOrdB,
1017  integer iPredS, integer iContrS, integer iPredH, integer iContrH,
1018  doublereal* pW, doublereal* pR, DriveCaller* pDC,
1019  doublereal dPF, flag f)
1020 : GPCDesigner(iNumOut, iNumIn, iOrdA, iOrdB, iPredS, iContrS, iPredH, iContrH,
1021  dPF),
1022 iDim(iNumOutputs*iPredStep),
1023 iTmpRows(iNumOutputs*(iPredStep-iPredHor)),
1024 iTmpCols(iNumInputs*iContrStep),
1025 pdW(pW),
1026 pdR(pR),
1027 Weight(pDC),
1028 pdM(NULL),
1029 pdInvP(NULL),
1030 f_armax(f),
1031 pInv(NULL)
1032 {
1033  ASSERT(pW != NULL);
1034  ASSERT(pW != NULL);
1035  ASSERT(pDC != NULL);
1036 
1037 #if !defined(USE_MESCHACH) && !defined(USE_LAPACK)
1038 #error "need LAPACK or Meschach for pseudoinverse"
1039 #endif /* !USE_MESCHACH && !USE_LAPACK */
1040  switch (1) { /* FIXME: mettere un flag */
1041 #ifdef USE_MESCHACH
1042  case 1:
1043  SAFENEWWITHCONSTRUCTOR(pInv,
1044  GPC_Meschach_QRinv,
1045  GPC_Meschach_QRinv(iTmpCols, iTmpCols));
1046  break;
1047 #endif /* USE_MESCHACH */
1048 #ifdef USE_LAPACK
1049  case 2:
1050  SAFENEWWITHCONSTRUCTOR(pInv,
1051  GPC_LAPACK_pinv,
1052  GPC_LAPACK_pinv(iTmpCols, iTmpCols));
1053  break;
1054 #endif /* USE_LAPACK */
1055  }
1056 
1057  if (pInv == NULL) {
1058  silent_cerr("unable to create GPCInv" << std::endl);
1059  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1060  }
1061 
1062  /* note that iPredHor == iContrStep */
1063 
1064  integer i = iDim*(iNumOutputs*iOrderA) /* A */
1065  +iDim*(iNumInputs*iOrderB) /* B */
1066  +iDim*(iNumInputs*iPredStep) /* P */
1067  +iTmpCols*iTmpCols /* M */
1068  +iTmpCols*iTmpRows /* InvP */
1069  +iNumInputs*(iNumOutputs*iOrderA) /* ac */
1070  +iNumInputs*(iNumInputs*iOrderB) /* bc */
1071  +iNumInputs*iTmpRows; /* md */
1072 
1073  if (f_armax) {
1074  i += iDim*(iNumOutputs*iOrderA) /* C */
1075  +iNumInputs*(iNumOutputs*iOrderA); /* cc */
1076  }
1077 
1078  SAFENEWARR(pdBase, doublereal, i);
1079 
1080  for (integer j = i; j-- > 0; ) {
1081  pdBase[j] = 0.;
1082  }
1083 
1084  pdA = pdBase;
1085  pdB = pdA+iDim*(iNumOutputs*iOrderA);
1086  pdP = pdB+iDim*(iNumInputs*iOrderB);
1087  pdM = pdP+iDim*(iNumInputs*iPredStep);
1088  pdInvP = pdM+iTmpCols*iTmpCols;
1089  pdac = pdInvP+iTmpCols*iTmpRows;
1090  pdbc = pdac+iNumInputs*(iNumOutputs*iOrderA);
1091  pdmd = pdbc+iNumInputs*(iNumInputs*iOrderB);
1092 
1093  pdPTmp = pdP+iDim*(iNumInputs*(iPredStep-iContrStep));
1094 
1095  if (f_armax) {
1096  pdC = pdmd+iNumInputs*iTmpRows;
1097  pdcc = pdC+iDim*(iNumOutputs*iOrderA);
1098  }
1099 }
1100 
1101 GPC::~GPC(void)
1102 {
1103  SAFEDELETEARR(pdW);
1104  SAFEDELETEARR(pdR);
1105 }
1106 
1107 static int
1108 make_m(integer ndimp, integer nrowp, integer ncolp, doublereal* P,
1109  integer nout, integer nin, integer s,
1110  doublereal* W, doublereal* R, doublereal lambda, doublereal* M)
1111 {
1112  for (integer j = ncolp; j-- > 0; ) {
1113  doublereal* m = M+ncolp*j;
1114  doublereal* pj = P+ndimp*j;
1115  for (integer i = ncolp; i-- > 0; ) {
1116  doublereal* mm = m+i;
1117  doublereal* pi = P+ndimp*i;
1118 
1119  mm[0] = 0.;
1120  for (integer k = s; k-- > 0; ) {
1121  doublereal* pik = pi+nout*k;
1122  doublereal* pjk = pj+nout*k;
1123  for (integer l = nout; l-- > 0; ) {
1124  /*
1125  * M[i+ncolp*j] =
1126  * P[nout*k+l+ndimp*i]
1127  * *P[nout*k+l+ndimp*j]*W[k];
1128  */
1129  mm[0] += pik[l]*pjk[l]*W[k];
1130  }
1131  }
1132  }
1133 
1134  /*
1135  * M[j+ncolp*j] += lambda*R[j/nin];
1136  */
1137  m[j] += lambda*R[j/nin];
1138  }
1139 
1140  return 0;
1141 }
1142 
1143 /*
1144  * moltiplica una matrice L, organizzata per righe, per una R, organizzata
1145  * per colonne, e trasposta, e mette il risultato in dest, organizzata per
1146  * righe
1147  */
1148 
1149 static int
1150 gpc_mult_t(integer ndiml, integer nrowl, integer ncoll, doublereal* L,
1151  integer ndimr, integer nrowr, integer ncolr, doublereal* R,
1152  integer ndimd, doublereal* dest)
1153 {
1154  for (integer i = nrowl; i-- > 0; ) {
1155  doublereal* di = dest+ndimd*i;
1156  doublereal* Li = L+ndiml*i;
1157 
1158  for (integer j = ncolr; j-- > 0; ) {
1159  doublereal* dij = di+j;
1160  doublereal* Rj = R+j;
1161 
1162  /*
1163  * dest[j+ndimd*i] = 0.;
1164  */
1165  dij[0] = 0.;
1166  for (integer k = ncoll; k-- > 0; ) {
1167  /*
1168  * dest[j+ndimd*i] +=
1169  * L[k+ndiml*i]*R[j+ndimr*k];
1170  */
1171  dij[0] += Li[k]*Rj[ndimr*k];
1172  }
1173  }
1174  }
1175 
1176  return 0;
1177 }
1178 
1179 void
1180 GPC::DesignControl(const doublereal* pdTheta,
1181  doublereal** ppda,
1182  doublereal** ppdb,
1183  doublereal** ppdm,
1184  doublereal** ppdc)
1185 {
1186  ASSERT(pdTheta != NULL);
1187 
1188  /*
1189  * si assume che pdTheta contenga le stime delle matrici a_1 .. a_pa,
1190  * b_0 .. b_pb disposte per righe, ovvero:
1191  * a_1(1,1), a_1(1,2), ..., a_1(1,m), a_2(1,1), ...,
1192  * a_pa(1,m),b_0(1,1),...
1193  */
1194 
1195  gpc_build_matrices(iDim, pdA,
1196  iDim, pdB,
1197  iDim, pdP,
1198  iDim, pdC, /* se f_armax == 0, pdC = NULL */
1199  iNumOutputs, iNumInputs,
1200  iPredStep, iOrderA, iOrderB,
1201  (doublereal*)pdTheta);
1202 
1203  /* M = P^T*W*P+R */
1204  make_m(iDim, iTmpRows, iTmpCols, pdPTmp,
1205  iNumOutputs, iNumInputs, iPredStep-iPredHor,
1206  pdW, pdR, Weight.dGet(), pdM);
1207 
1208  /* l'inversore sa come invertire, e mette l'inversa in pdM */
1209  integer info = pInv->Inv(iTmpCols, iTmpCols, iTmpCols, pdM);
1210 
1211  /* M^-1*P^T */
1212  gpc_mult_t(iTmpCols, iTmpCols, iTmpCols, pdM,
1213  iDim, iTmpRows, iTmpCols, pdPTmp,
1214  iTmpRows, pdInvP);
1215 
1216  if (info < 0) {
1217  silent_cerr("GPC::DesignControl(): illegal value in "
1218  << -info << "-th argument of dgesvd()" << std::endl);
1219  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1220  } else if (info > 0) {
1221  silent_cerr("GPC::DesignControl(): error in dgesvd()" << std::endl);
1222  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1223  } /* else: OK */
1224 
1225  /* recupero delle matrici (pinv(P) e' organizzata per righe) */
1226 
1227  /* copia le ultime righe dell'inversa di p, che sta in pdPTmp, in md */
1228  doublereal* p = pdInvP+iTmpRows*(iNumInputs*(iContrStep-1));
1229 
1230 #if 0 /* FIXME: here weight matrices are applied to the pseudo-inverse */
1231  for (integer i = iTmpRows*iNumInputs; i-- > 0; ) {
1232  pdmd[i] = p[i];
1233  }
1234 #else /* !0 */
1235  for (integer i = iNumInputs; i-- > 0; ) {
1236  doublereal* pi = p+iTmpRows*i;
1237  doublereal* pmi = pdmd+iTmpRows*i;
1238 
1239  for (integer k = iPredStep-iPredHor; k-- > 0; ) {
1240  doublereal* pik = pi+iNumOutputs*k;
1241  doublereal* pmik = pmi+iNumOutputs*k;
1242 
1243  for (integer l = iNumOutputs; l-- > 0; ) {
1244  /*
1245  * pdmd[iTmpRows*i+iNumOutputs*k+l] =
1246  * p[iTmpRows*i+iNumOutputs*k+l]*pdW[k];
1247  */
1248  pmik[l] = pik[l]*pdW[k];
1249  }
1250  }
1251  }
1252 #endif /* !0 */
1253 
1254  /* anche ac, bc (e cc) vengono organizzate per righe */
1255  doublereal cc = dPeriodicFactor;
1256  for (integer i = iNumInputs; i-- > 0; ) {
1257  doublereal* pm = pdmd+iTmpRows*i;
1258 
1259  /* ac = -md*A */
1260  doublereal* p = pdac+i*iNumOutputs*iOrderA;
1261  for (integer j = iNumOutputs*iOrderA; j-- > 0; ) {
1262  doublereal* pa = pdA+iDim*j;
1263  p[j] = pm[j]*cc;
1264  for (integer k = iTmpRows; k-- > 0; ) {
1265  /* p[j] -= pdmd[iTmpRows*i+k]*pdA[k+iDim*j]; */
1266  p[j] -= pm[k]*pa[k];
1267  }
1268  }
1269 
1270  /* bc = -md*B */
1271  p = pdbc+i*iNumInputs*iOrderB;
1272  for (integer j = iNumInputs*iOrderB; j-- > 0; ) {
1273  doublereal* pb = pdB+iDim*j;
1274  p[j] = 0.;
1275  for (integer k = iTmpRows; k-- > 0; ) {
1276  /* p[j] -= pdmd[iTmpRows*i+k]*pdB[k+iDim*j]; */
1277  p[j] -= pm[k]*pb[k];
1278  }
1279  }
1280 
1281  if (f_armax) {
1282  /* cc = -md*C */
1283  p = pdcc+i*iNumOutputs*iOrderA;
1284  for (integer j = iNumOutputs*iOrderA; j-- > 0; ) {
1285  doublereal* pc = pdC+iDim*j;
1286  p[j] = 0.;
1287  for (integer k = iTmpRows; k-- > 0; ) {
1288  /*
1289  * p[j] -=
1290  * pdmd[iTmpRows*i+k]*pdC[k+iDim*j];
1291  */
1292  p[j] -= pm[k]*pc[k];
1293  }
1294  }
1295  }
1296  }
1297 
1298  /* linka i puntatori alle matrici, se richiesto */
1299  GPCDesigner::DesignControl(NULL, ppda, ppdb, ppdm, ppdc);
1300 }
1301 #endif /* USE_DBC */
1302 
1303 /* GPC - end */
1304 
1305 /*
1306  * Test of matrix computation
1307  */
1308 
1309 #ifdef TEST_DPC
1310 
1311 int main(void)
1312 {
1313 #ifdef USE_DBC
1314  const integer nout = 2;
1315  const integer nin = 2;
1316  const integer s = 4;
1317  const integer pa = 2;
1318  const integer pb = 2;
1319 
1320  doublereal d[nout*nout*pa*s+nout*nin*pb*s+nout*s*nin*s+nout*nout*pa*s];
1321  doublereal theta[nout*nout*pa+nout*nin*(pb+1)+nout*nout*pa] = {
1322  2., 0.,-1., 0., 1., 0.,-1., 0., 1., 0., -2., 0., 1., 0.,
1323  0., 2., 0.,-1., 0., 1., 0.,-1., 0., 1., 0.,-2., 0., 1.
1324  };
1325 
1326  /*
1327  for (integer i = 0; i < nout*nout*pa+nout*nin*(pb+1)+nout*nout*pa; i++) {
1328  theta[i] = 1.;
1329  }
1330  */
1331 
1332  gpc_build_matrices(nout*s, d,
1333  nout*s, d+nout*nout*pa*s,
1334  nout*s, d+nout*nout*pa*s+nout*nin*pb*s,
1335  nout*s, d+nout*nout*pa*s+nout*nin*pb*s+nout*s*nin*s,
1336  nout, nin,
1337  s, pa, pb,
1338  theta);
1339 
1340  doublereal* pA = d;
1341  silent_cout("A:" << std::endl);
1342  for (integer i = 0; i < nout*s; i++) {
1343  for (integer j = 0; j < nout*pa; j++) {
1344  silent_cout(std::setw(8) << pA[i+nout*s*j]);
1345  }
1346  silent_cout(std::endl);
1347  }
1348  doublereal* pB = d+nout*nout*pa*s;
1349  silent_cout("B:" << std::endl);
1350  for (integer i = 0; i < nout*s; i++) {
1351  for (integer j = 0; j < nin*pb; j++) {
1352  silent_cout(std::setw(8) << pB[i+nout*s*j]);
1353  }
1354  silent_cout(std::endl);
1355  }
1356  doublereal* pP = d+nout*nout*pa*s+nout*s*nin*pb;
1357  silent_cout("P:" << std::endl);
1358  for (integer i = 0; i < nout*s; i++) {
1359  for (integer j = 0; j < nin*s; j++) {
1360  silent_cout(std::setw(8) << pP[i+nout*s*j]);
1361  }
1362  silent_cout(std::endl);
1363  }
1364  doublereal* pC = d+nout*nout*pa*s+nout*s*nin*pb+nout*s*nin*s;
1365  silent_cout("C:" << std::endl);
1366  for (integer i = 0; i < nout*s; i++) {
1367  for (integer j = 0; j < nout*pa; j++) {
1368  silent_cout(std::setw(8) << pC[i+nout*s*j]);
1369  }
1370  silent_cout(std::endl);
1371  }
1372 
1373  integer q = s/2;
1374  integer tmprow = (s-q)*nout;
1375  integer tmpcol = q*nin;
1376 #ifdef USE_MESCHACH
1377  GPC_Meschach_QRinv inv(tmprow, tmpcol);
1378 #else /* !USE_MESCHACH */
1379 #error "Need Meschach"
1380 #endif /* !USE_MESCHACH */
1381  doublereal* pT = pP+s*nout*(s-q)*nin;
1382  inv.Inv(s*nout, tmprow, tmpcol, pT);
1383 
1384  silent_cout("Inv(P'):" << std::endl);
1385  for (integer i = 0; i < tmpcol; i++) {
1386  for (integer j = 0; j < tmprow; j++) {
1387  silent_cout(std::setw(12) << pT[tmprow*i+j]);
1388  }
1389  silent_cout(std::endl);
1390  }
1391 
1392  DeadBeat db(nout, nin,
1393  pa, pb,
1394  s, q, 1, 0.);
1395 
1396  doublereal* pac;
1397  doublereal* pbc;
1398  doublereal* pcc;
1399  doublereal* pmd;
1400  db.DesignControl(theta, &pac, &pbc, &pmd, &pcc);
1401 
1402  silent_cout("Ac:" << std::endl);
1403  for (integer i = 0; i < nin; i++) {
1404  for (integer j = 0; j < nout*pa; j++) {
1405  silent_cout(std::setw(12) << pac[i*nout*pa+j]);
1406  }
1407  silent_cout(std::endl);
1408  }
1409  silent_cout("Bc:" << std::endl);
1410  for (integer i = 0; i < nin; i++) {
1411  for (integer j = 0; j < nin*pb; j++) {
1412  silent_cout(std::setw(12) << pbc[i*nin*pb+j]);
1413  }
1414  silent_cout(std::endl);
1415  }
1416  silent_cout("Cc:" << std::endl);
1417  for (integer i = 0; i < nin; i++) {
1418  for (integer j = 0; j < nout*pa; j++) {
1419  silent_cout(std::setw(12) << pcc[i*nout*pa+j]);
1420  }
1421  silent_cout(std::endl);
1422  }
1423  silent_cout("Md:" << std::endl);
1424  for (integer i = 0; i < nin; i++) {
1425  for (integer j = 0; j < tmprow; j++) {
1426  silent_cout(std::setw(12) << pmd[i*tmprow+j]);
1427  }
1428  silent_cout(std::endl);
1429  }
1430 #else /* !USE_DBC */
1431  silent_cerr("cannot use GPC/Deadbeat" << std::endl);
1432 #endif /* !USE_DBC */
1433  return 0;
1434 }
1435 
1436 #endif /* TEST_DPC */
1437 
GPC::pdW
doublereal * pdW
Definition: gpc.h:280
GPC::Weight
DriveOwner Weight
Definition: gpc.h:282
GPCDesigner::~GPCDesigner
virtual ~GPCDesigner(void)
Definition: gpc.cc:811
GPC::pdInvP
doublereal * pdInvP
Definition: gpc.h:285
flag
long int flag
Definition: mbdyn.h:43
GPCDesigner::iPredStep
integer iPredStep
Definition: gpc.h:137
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
gpc_zero
static int gpc_zero(integer size, doublereal *v)
Definition: gpc.cc:258
gpc_build_matrices
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)
Definition: gpc.cc:284
main
int main(int argc, char *argv[])
Definition: dgeequtest.cc:74
GPCDesigner::pdA
doublereal * pdA
Definition: gpc.h:146
GPC_LAPACK_pinv::n
integer n
Definition: gpc.h:78
SAFEDELETEARR
#define SAFEDELETEARR(pnt)
Definition: mynewmem.h:713
GPC_LAPACK_pinv::pdWork
doublereal * pdWork
Definition: gpc.h:88
GPC_LAPACK_pinv
Definition: gpc.h:75
gpc_mcopy
static int gpc_mcopy(integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
Definition: gpc.cc:212
GPC::pInv
GPCInv * pInv
Definition: gpc.h:289
GPCDesigner::pdBase
doublereal * pdBase
Definition: gpc.h:144
GPCDesigner::pdac
doublereal * pdac
Definition: gpc.h:151
GPC::GPC
GPC(integer iNumOut, integer iNumIn, integer iOrdA, integer iOrdB, integer iPredS, integer iContrS, integer iPredH, integer iContrH, doublereal *pW, doublereal *pR, DriveCaller *pDC, doublereal dPF, flag f)
GPC::pdM
doublereal * pdM
Definition: gpc.h:284
GPC_LAPACK_pinv::pdVt
doublereal * pdVt
Definition: gpc.h:87
NO_OP
#define NO_OP
Definition: myassert.h:74
GPCDesigner::dPeriodicFactor
doublereal dPeriodicFactor
Definition: gpc.h:142
gpc_addmul
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
GPCInv::pdBase
doublereal * pdBase
Definition: gpc.h:55
GPCInv::Inv
virtual integer Inv(integer ndima, integer nrowa, integer ncola, doublereal *a)=0
GPC::pdPTmp
doublereal * pdPTmp
Definition: gpc.h:278
grad::fabs
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
GPC::~GPC
virtual ~GPC(void)
GPC_LAPACK_pinv::pdS
doublereal * pdS
Definition: gpc.h:85
gpc_pinv
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)
Definition: gpc.cc:518
GPC_LAPACK_pinv::iMax
integer iMax
Definition: gpc.h:81
GPCDesigner::iOrderB
integer iOrderB
Definition: gpc.h:135
GPC::iTmpRows
integer iTmpRows
Definition: gpc.h:275
GPC::iTmpCols
integer iTmpCols
Definition: gpc.h:276
GPCInv
Definition: gpc.h:53
dgesvd
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)
grad::Inv
Matrix< T, 2, 2 > Inv(const Matrix< T, 2, 2 > &A)
Definition: matvec.h:3282
GPC::iDim
integer iDim
Definition: gpc.h:274
GPC_LAPACK_pinv::m
integer m
Definition: gpc.h:77
GPC_LAPACK_pinv::pdU
doublereal * pdU
Definition: gpc.h:86
GPCDesigner::iOrderA
integer iOrderA
Definition: gpc.h:134
gpc_mul
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
GPCDesigner::pdC
doublereal * pdC
Definition: gpc.h:149
gpc_set
static int gpc_set(integer size, doublereal *v, const doublereal d)
Definition: gpc.cc:271
GPC_LAPACK_pinv::Inv
integer Inv(integer ndima, integer nrowa, integer ncola, doublereal *a)
Definition: gpc.cc:662
GPC_LAPACK_pinv::iMin
integer iMin
Definition: gpc.h:80
GPCDesigner::iNumOutputs
integer iNumOutputs
Definition: gpc.h:132
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
GPCDesigner::pdbc
doublereal * pdbc
Definition: gpc.h:152
GPC::DesignControl
void DesignControl(const doublereal *pdTheta, doublereal **ppda=NULL, doublereal **ppdb=NULL, doublereal **ppdm=NULL, doublereal **ppdc=NULL)
SAFENEWWITHCONSTRUCTOR
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
GPCDesigner::iNumInputs
integer iNumInputs
Definition: gpc.h:133
GPC::pdR
doublereal * pdR
Definition: gpc.h:281
GPCDesigner::pdP
doublereal * pdP
Definition: gpc.h:148
c
static std::stack< cleanup * > c
Definition: cleanup.cc:59
GPCDesigner::pdB
doublereal * pdB
Definition: gpc.h:147
GPC_LAPACK_pinv::~GPC_LAPACK_pinv
~GPC_LAPACK_pinv(void)
Definition: gpc.cc:652
GPCDesigner::GPCDesigner
GPCDesigner(integer iNumOut, integer iNumIn, integer iOrdA, integer iOrdB, integer iPredS, integer iContrS, integer iPredH, integer iContrH, doublereal dPF)
Definition: gpc.cc:781
GPCInv::~GPCInv
virtual ~GPCInv(void)
Definition: gpc.cc:588
DriveOwner::dGet
doublereal dGet(const doublereal &dVar) const
Definition: drive.cc:664
GPCDesigner::pdcc
doublereal * pdcc
Definition: gpc.h:154
SAFENEWARR
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
gpc_add_mul
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)
Definition: gpc.cc:145
a
static const doublereal a
Definition: hfluid_.h:289
GPCDesigner::pdmd
doublereal * pdmd
Definition: gpc.h:153
gpc_mcopy_t
static int gpc_mcopy_t(integer ndims, integer nrows, integer ncols, const doublereal *s, integer ndimd, doublereal *d)
Definition: gpc.cc:235
GPCDesigner::DesignControl
virtual void DesignControl(const doublereal *, doublereal **ppda=NULL, doublereal **ppdb=NULL, doublereal **ppdm=NULL, doublereal **ppdc=NULL)
Definition: gpc.cc:818
GPC_LAPACK_pinv::iWork
integer iWork
Definition: gpc.h:83
doublereal
double doublereal
Definition: colamd.c:52
GPCDesigner::iContrStep
integer iContrStep
Definition: gpc.h:138
integer
long int integer
Definition: colamd.c:51
GPC::f_armax
flag f_armax
Definition: gpc.h:287
GPCInv::GPCInv
GPCInv(void)
Definition: gpc.cc:582
GPC_LAPACK_pinv::GPC_LAPACK_pinv
GPC_LAPACK_pinv(integer n, integer m)
Definition: gpc.cc:600
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
GPCDesigner::iPredHor
integer iPredHor
Definition: gpc.h:139