MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
genfilt.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/elec/genfilt.cc,v 1.57 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 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 #include "genfilt.h"
35 
36 #ifdef USE_LAPACK
37 extern "C" int
38 __FC_DECL__(dgebal)(char *JOB, integer *N, doublereal *pdA, integer *LDA,
39  integer *ILO, integer *IHI, doublereal *SCALE, integer *INFO);
40 extern "C" int
41 __FC_DECL__(dggbal)(char *JOB, integer *N, doublereal *pdA, integer *LDA,
42  doublereal *pdB, integer *LDB, integer *ILO, integer *IHI,
43  doublereal *LSCALE, doublereal *RSCALE, doublereal *WORK,
44  integer *INFO);
45 #endif // USE_LAPACK
46 
47 /* GenelStateSpaceSISO - begin */
48 
50  const DofOwner* pDO,
51  const ScalarDof& y,
52  ScalarValue* u,
53  unsigned int Order,
54  doublereal* pE,
55  doublereal* pA,
56  doublereal* pB,
57  doublereal* pC,
58  doublereal D,
59  bool bBalance,
60  doublereal *pdX0,
61  doublereal *pdXP0,
62  flag fOutput)
63 : Elem(uLabel, fOutput),
64 Genel(uLabel, pDO, fOutput),
65 SD_y(y), SV_u(u),
66 iNumDofs(Order),
67 pdE(pE), pdA(pA), pdB(pB), pdC(pC), dD(D),
68 pdX(0), pdXP(0)
69 {
70  ASSERT(Order > 0);
71  ASSERT(pdA != 0);
72  ASSERT(pdB != 0);
73  ASSERT(pdC != 0);
74  ASSERT(SD_y.iOrder == 0);
75  DEBUGCOUT("GenelStateSpaceSISO " << uLabel
76  << ", NumDofs: " << iNumDofs << std::endl);
77 
78 #ifdef USE_LAPACK
79  // try balancing the matrix?
80  if (bBalance) {
81  char JOB = 'S';
82  integer N = Order;
83  integer LDA = Order;
84  integer LDB = Order;
85  integer ILO;
86  integer IHI;
87  integer INFO;
88 
89  std::vector<doublereal> RSCALE(Order);
90  std::vector<doublereal> LSCALE(Order);
91 
92  if (pdE != 0) {
93  std::vector<doublereal> WORK(6*Order);
94 
95  (void)__FC_DECL__(dggbal)(&JOB, &N, pdA, &LDA,
96  pdE, &LDB, &ILO, &IHI,
97  &LSCALE[0], &RSCALE[0], &WORK[0], &INFO);
98 
99  } else {
100  (void)__FC_DECL__(dgebal)(&JOB, &N, pdA, &LDA,
101  &ILO, &IHI, &RSCALE[0], &INFO);
102 
103  for (unsigned i = 0; i < Order; i++) {
104  LSCALE[i] = 1./RSCALE[i];
105  }
106  }
107 
108  if (INFO != 0) {
109  silent_cout("GenelStateSpaceSISO(" << uLabel << "): "
110  "balancing failed (ignored)" << std::endl);
111 
112  } else {
113  for (unsigned i = 0; i < Order; i++) {
114  pdB[i] *= RSCALE[i];
115  pdC[i] *= LSCALE[i];
116  }
117  }
118  }
119 #endif // USE_LAPACK
120 
121  SAFENEWARR(pdX, doublereal, 2*Order);
122  pdXP = pdX + Order;
123 
124  for (unsigned i = 0; i < 2*Order; i++) {
125  pdX[i] = 0.;
126  }
127 
128  if (pdX0) {
129  for (unsigned i = 0; i < Order; i++) {
130  pdX[i] = pdX0[i];
131  }
132 
133  if (pdXP0) {
134  for (unsigned i = 0; i < Order; i++) {
135  pdXP[i] = pdXP0[i];
136  }
137  }
138  }
139 }
140 
142 {
143  if (pdX != 0) {
145  }
146 
147  if (pdC != 0) {
149  }
150 
151  if (pdB != 0) {
153  }
154 
155  if (pdA != 0) {
157  }
158 
159  if (pdE != 0) {
161  }
162 
163  if (SV_u != 0) {
164  SAFEDELETE(SV_u);
165  }
166 }
167 
168 unsigned int
170 {
171  return iNumDofs;
172 }
173 
174 /* esegue operazioni sui dof di proprieta' dell'elemento */
176 GenelStateSpaceSISO::GetDofType(unsigned int i) const
177 {
178  ASSERT(i < iNumDofs);
179  return DofOrder::DIFFERENTIAL;
180 }
181 
182 /* Scrive il contributo dell'elemento al file di restart */
183 std::ostream&
184 GenelStateSpaceSISO::Restart(std::ostream& out) const
185 {
186  return out << "GenelStateSpaceSISO: not implemented yet!" << std::endl;
187 }
188 
189 /* Dimensioni del workspace */
190 void
191 GenelStateSpaceSISO::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
192 {
193  *piNumRows = iNumDofs + 1;
194  *piNumCols = iNumDofs + 1;
195 
196  // inputs may contribute to the Jacobian matrix
197  if (dynamic_cast<ScalarDofValue *>(SV_u) != 0) {
198  *piNumCols += 1;
199  }
200 }
201 
202 /* assemblaggio jacobiano */
205  doublereal dCoef,
206  const VectorHandler& /* XCurr */ ,
207  const VectorHandler& /* XPrimeCurr */ )
208 {
209  DEBUGCOUT("Entering GenelStateSpaceSISO::AssJac()" << std::endl);
210 
211  FullSubMatrixHandler& WM = WorkMat.SetFull();
212  integer iNumRows = 0;
213  integer iNumCols = 0;
214  WorkSpaceDim(&iNumRows, &iNumCols);
215  WM.ResizeReset(iNumRows, iNumCols);
216 
217  integer iRowIndex_y = SD_y.pNode->iGetFirstRowIndex() + 1;
218  integer iColIndex_y = SD_y.pNode->iGetFirstColIndex() + 1;
219  integer iFirstIndex = iGetFirstIndex();
220 
221  WM.PutRowIndex(iNumRows, iRowIndex_y);
222  WM.PutColIndex(iNumCols, iColIndex_y);
223 
224  // inputs may contribute to the Jacobian matrix
225  ScalarDofValue *SDV_u = dynamic_cast<ScalarDofValue *>(SV_u);
226  if (SDV_u != 0) {
227  ScalarDof& SD_u = dynamic_cast<ScalarDof &>(*SV_u);
228  integer iColIndex_u = SD_u.pNode->iGetFirstRowIndex() + 1;
229  integer iIdx_u = iNumCols - 1;
230 
231  WM.PutColIndex(iIdx_u, iColIndex_u);
232 
233  doublereal dd;
234  if (SD_u.iOrder == 0) {
235  dd = dCoef;
236  } else {
237  dd = 1.;
238  }
239 
240  doublereal *pdb = pdB - 1;
241  for (unsigned int i = iNumDofs; i > 0; i--) {
242  WM.PutCoef(i, iIdx_u, pdb[i]*dd);
243  }
244  }
245 
246  WM.PutCoef(iNumRows, iNumCols, dCoef);
247 
248  doublereal* pda = pdA + iNumDofs*iNumDofs - 1;
249  doublereal* pdc = pdC - 1;
250  if (pdE) {
251  doublereal* pde = pdE + iNumDofs*iNumDofs - 1;
252 
253  for (unsigned int i = iNumDofs; i > 0; i--) {
254  WM.PutRowIndex(i, iFirstIndex + i);
255  WM.PutColIndex(i, iFirstIndex + i);
256  WM.PutCoef(iNumRows, i, -pdc[i]*dCoef);
257  pde -= iNumDofs;
258  pda -= iNumDofs;
259  for (unsigned int j = iNumDofs; j > 0; j--) {
260  /* Attenzione: si assume A orientata per righe:
261  * a_11, a_12, ..., a_1n, a_21, ..., a_2n, ..., a_nn */
262  WM.PutCoef(i, j, pde[j] - pda[j]*dCoef);
263  }
264  }
265 
266  } else {
267  for (unsigned int i = iNumDofs; i > 0; i--) {
268  WM.PutRowIndex(i, iFirstIndex + i);
269  WM.PutColIndex(i, iFirstIndex + i);
270  WM.PutCoef(iNumRows, i, -pdc[i]*dCoef);
271  pda -= iNumDofs;
272  for (unsigned int j = iNumDofs; j > 0; j--) {
273  /* Attenzione: si assume A orientata per righe:
274  * a_11, a_12, ..., a_1n, a_21, ..., a_2n, ..., a_nn */
275  WM.PutCoef(i, j, -pda[j]*dCoef);
276  }
277  WM.IncCoef(i, i, 1.);
278  }
279  }
280 
281  return WorkMat;
282 }
283 
284 
285 /* assemblaggio residuo */
288  doublereal /* dCoef */,
289  const VectorHandler& XCurr,
290  const VectorHandler& XPrimeCurr)
291 {
292  DEBUGCOUT("Entering GenelStateSpaceSISO::AssRes()" << std::endl);
293 
294  integer iNumRows = 0;
295  integer iNumCols = 0;
296  WorkSpaceDim(&iNumRows, &iNumCols);
297  WorkVec.ResizeReset(iNumRows);
298 
299  integer iRowIndex_y = SD_y.pNode->iGetFirstRowIndex()+1;
300  integer iFirstIndex = iGetFirstIndex();
301 
302  doublereal y = SD_y.pNode->dGetX();
303  doublereal u = SV_u->dGetValue();
304 
305  WorkVec.PutRowIndex(iNumRows, iRowIndex_y);
306 
307  doublereal* pdx = pdX - 1;
308  doublereal* pdxp = pdXP - 1;
309  doublereal* pdc = pdC - 1;
310  doublereal d = dD*u - y;
311  for (unsigned int i = iNumDofs; i > 0; i--) {
312  WorkVec.PutRowIndex(i, iFirstIndex + i);
313  pdx[i] = XCurr(iFirstIndex + i);
314  pdxp[i] = XPrimeCurr(iFirstIndex + i);
315  d += pdc[i]*pdx[i];
316  }
317 
318  WorkVec.PutCoef(iNumRows, d);
319 
320  doublereal* pda = pdA + iNumDofs*iNumDofs;
321  doublereal* pdb = pdB;
322  pdxp = pdXP;
323  pdx = pdX;
324  if (pdE) {
325  doublereal *pde = pdE + iNumDofs*iNumDofs;
326  for (unsigned int i = iNumDofs; i-- > 0; ) {
327  d = pdb[i]*u;
328  pde -= iNumDofs;
329  pda -= iNumDofs;
330  for (unsigned int j = iNumDofs; j-- > 0; ) {
331  d += pda[j]*pdx[j] - pde[j]*pdxp[j];
332  }
333  WorkVec.PutCoef(i + 1, d);
334  }
335 
336  } else {
337  for (unsigned int i = iNumDofs; i-- > 0; ) {
338  d = pdb[i]*u - pdxp[i];
339  pda -= iNumDofs;
340  for (unsigned int j = iNumDofs; j-- > 0; ) {
341  d += pda[j]*pdx[j];
342  }
343  WorkVec.PutCoef(i + 1, d);
344  }
345  }
346 
347  return WorkVec;
348 }
349 
350 void
354 {
355  integer iFirstIndex = iGetFirstIndex() + 1;
356 
357  if (pdE == 0) {
358  doublereal u = SV_u->dGetValue();
359  doublereal* pda = pdA + iNumDofs*iNumDofs;
360  doublereal* pdb = pdB;
361 
362  for (unsigned int i = iNumDofs; i-- > 0; ) {
363  pdXP[i] = pdb[i]*u;
364  pda -= iNumDofs;
365  for (unsigned int j = iNumDofs; j-- > 0; ) {
366  pdXP[i] += pda[j]*pdX[j];
367  }
368  }
369  }
370 
371  for (unsigned i = 0; i < iNumDofs; i++) {
372  X(iFirstIndex + i) = pdX[i];
373  XP(iFirstIndex + i) = pdXP[i];
374  }
375 }
376 
377 /*
378  * output; si assume che ogni tipo di elemento sappia, attraverso
379  * l'OutputHandler, dove scrivere il proprio output
380  */
381 void
383 {
384  if (bToBeOutput()) {
385  std::ostream &out(OH.Genels());
386  out << std::setw(8) << GetLabel();
387  for (unsigned int i = 0; i < iNumDofs; i++) {
388  out << " " << pdX[i];
389  }
390  for (unsigned int i = 0; i < iNumDofs; i++) {
391  out << " " << pdXP[i];
392  }
393  out << "\n";
394  }
395 }
396 
397 void
399  std::vector<const Node *>& connectedNodes) const {
400  unsigned iNodes = 1;
401  if (dynamic_cast<NodeDof *>(SV_u)) {
402  iNodes++;
403  }
404  connectedNodes.resize(iNodes);
405  connectedNodes[0] = SD_y.pNode;
406  if (dynamic_cast<NodeDof *>(SV_u)) {
407  connectedNodes[1] = dynamic_cast<NodeDof *>(SV_u)->pNode;
408  }
409 }
410 
411 /* GenelStateSpaceSISO - end */
412 
413 
414 /* GenelStateSpaceMIMO - begin */
415 
417  const DofOwner* pDO,
418  unsigned int iNumOut,
419  const ScalarDof* y,
420  std::vector<ScalarValue *>& u,
421  unsigned int Order,
422  doublereal* pE,
423  doublereal* pA,
424  doublereal* pB,
425  doublereal* pC,
426  doublereal* pD,
427  bool bBalance,
428  doublereal *pdX0,
429  doublereal *pdXP0,
430  flag fOutput)
431 : Elem(uLabel, fOutput),
432 Genel(uLabel, pDO, fOutput),
433 iNumOutputs(iNumOut), iNumInputs(u.size()),
434 pvSD_y(const_cast<ScalarDof *>(y)), SV_u(u),
435 iNumDofs(Order),
436 pdE(pE), pdA(pA), pdB(pB), pdC(pC), pdD(pD),
437 pdX(0), pdXP(0)
438 {
439  ASSERT(iNumDofs > 0);
440  ASSERT(iNumOutputs > 0);
441  ASSERT(pvSD_y != 0);
442  for (int i = iNumOutputs; i-- > 0; ) {
443  ASSERT(pvSD_y[i].iOrder == 0);
444  }
445  ASSERT(iNumInputs > 0);
446  ASSERT(pdA != 0);
447  ASSERT(pdB != 0);
448  ASSERT(pdC != 0);
449  DEBUGCOUT("GenelStateSpaceMIMO " << uLabel
450  << ", NumDofs: " << iNumDofs << std::endl);
451 
452 #ifdef USE_LAPACK
453  // try balancing the matrix?
454  if (bBalance) {
455  int rc;
456  char JOB = 'S';
457  integer N = Order;
458  integer LDA = Order;
459  integer LDB = Order;
460  integer ILO;
461  integer IHI;
462  integer INFO;
463 
464  std::vector<doublereal> RSCALE(Order);
465  std::vector<doublereal> LSCALE(Order);
466 
467  if (pdE != 0) {
468  std::vector<doublereal> WORK(6*Order);
469 
470  rc = __FC_DECL__(dggbal)(&JOB, &N, pdA, &LDA,
471  pdE, &LDB, &ILO, &IHI,
472  &LSCALE[0], &RSCALE[0], &WORK[0], &INFO);
473 
474  } else {
475  rc = __FC_DECL__(dgebal)(&JOB, &N, pdA, &LDA,
476  &ILO, &IHI, &RSCALE[0], &INFO);
477 
478  for (unsigned i = 0; i < Order; i++) {
479  LSCALE[i] = 1./RSCALE[i];
480  }
481  }
482 
483  if (INFO != 0) {
484  silent_cout("GenelStateSpaceMIMO(" << uLabel << "): "
485  "balancing failed (ignored)" << std::endl);
486 
487  } else {
488  doublereal *pd = pdB;
489 
490  for (unsigned i = 0; i < Order; i++) {
491  doublereal d = RSCALE[i];
492  for (unsigned j = 0; j < iNumInputs; j++) {
493  *pd++ *= d;
494  }
495  }
496 
497  pd = pdC;
498  for (unsigned j = 0; j < iNumOutputs; j++) {
499  for (unsigned i = 0; i < Order; i++) {
500  *pd++ *= LSCALE[i];
501  }
502  }
503  }
504  }
505 #endif // USE_LAPACK
506 
507  SAFENEWARR(pdX, doublereal, 2*Order);
508  pdXP = pdX + Order;
509 
510  for (unsigned i = 0; i < 2*Order; i++) {
511  pdX[i] = 0.;
512  }
513 
514  if (pdX0) {
515  for (unsigned i = 0; i < Order; i++) {
516  pdX[i] = pdX0[i];
517  }
518 
519  if (pdXP0) {
520  for (unsigned i = 0; i < Order; i++) {
521  pdXP[i] = pdXP0[i];
522  }
523  }
524  }
525 }
526 
528 {
529  if (pdX != 0) {
531  }
532 
533  if (pdD != 0) {
535  }
536 
537  if (pdC != 0) {
539  }
540 
541  if (pdB != 0) {
543  }
544 
545  if (pdA != 0) {
547  }
548 
549  if (pdE != 0) {
551  }
552 
553  for (std::vector<ScalarValue *>::iterator i = SV_u.begin();
554  i != SV_u.end(); ++i)
555  {
556  delete *i;
557  }
558 
559  if (pvSD_y != 0) {
561  }
562 }
563 
564 unsigned int GenelStateSpaceMIMO::iGetNumDof(void) const
565 {
566  return iNumDofs;
567 }
568 
569 /* esegue operazioni sui dof di proprieta' dell'elemento */
571 {
572  ASSERT(i < iNumDofs);
573  return DofOrder::DIFFERENTIAL;
574 }
575 
576 /* Scrive il contributo dell'elemento al file di restart */
577 std::ostream& GenelStateSpaceMIMO::Restart(std::ostream& out) const
578 {
579  return out << "GenelStateSpaceMIMO: not implemented yet!" << std::endl;
580 }
581 
582 /* Dimensioni del workspace */
584  integer* piNumCols) const
585 {
586  *piNumRows = iNumDofs + iNumOutputs;
587  *piNumCols = iNumDofs + iNumOutputs;
588 
589  // inputs may contribute to the Jacobian matrix
590  for (unsigned int j = iNumInputs; j-- > 0; ) {
591  if (dynamic_cast<ScalarDofValue *>(SV_u[j]) != 0) {
592  *piNumCols += 1;
593  }
594  }
595 }
596 
597 /* assemblaggio jacobiano */
600  doublereal dCoef,
601  const VectorHandler& /* XCurr */ ,
602  const VectorHandler& /* XPrimeCurr */ )
603 {
604  DEBUGCOUT("Entering GenelStateSpaceMIMO::AssJac()" << std::endl);
605 
606  FullSubMatrixHandler& WM = WorkMat.SetFull();
607  integer iNumRows = 0;
608  integer iNumCols = 0;
609  WorkSpaceDim(&iNumRows, &iNumCols);
610  WM.ResizeReset(iNumRows, iNumCols);
611 
612  // inputs may contribute to the Jacobian matrix
613  integer iIdx_u = iNumDofs + iNumOutputs;
614  for (unsigned int j = 0; j < iNumInputs; j++) {
615  ScalarDofValue *SDV_u = dynamic_cast<ScalarDofValue *>(SV_u[j]);
616  if (SDV_u != 0) {
617  ScalarDof& SD_u = dynamic_cast<ScalarDof &>(*SV_u[j]);
618  integer iColIndex_u = SD_u.pNode->iGetFirstRowIndex() + 1;
619  iIdx_u += 1;
620  WM.PutColIndex(iIdx_u, iColIndex_u);
621 
622  doublereal dd;
623  if (SD_u.iOrder == 0) {
624  dd = dCoef;
625  } else {
626  dd = 1.;
627  }
628 
629  doublereal *pdb = &pdB[j];
630  for (unsigned int i = 1; i <= iNumDofs; i++) {
631  WM.PutCoef(i, iIdx_u, pdb[0]*dd);
632  pdb += iNumInputs;
633  }
634  }
635  }
636 
637  integer iFirstIndex = iGetFirstIndex();
638  doublereal* pdc = pdC + iNumOutputs*iNumDofs - 1;
639  for (unsigned int i = iNumOutputs; i > 0; i--) {
640  integer iRowIndex_y = pvSD_y[i - 1].pNode->iGetFirstRowIndex() + 1;
641  integer iColIndex_y = pvSD_y[i - 1].pNode->iGetFirstColIndex() + 1;
642 
643  WM.PutRowIndex(iNumDofs + i, iRowIndex_y);
644  WM.PutColIndex(iNumDofs + i, iColIndex_y);
645  // 1 sulla diagonale
646  WM.PutCoef(iNumDofs + i, iNumDofs + i, dCoef);
647 
648  pdc -= iNumDofs;
649  for (unsigned int j = iNumDofs; j > 0; j--) {
650  /* Attenzione: si assume C orientata per righe:
651  * c_11, c_12, ..., c_1n, c_21, ..., c_2n, ..., c_nn */
652  WM.PutCoef(iNumDofs + i, j, -pdc[j]*dCoef);
653  }
654  }
655 
656  doublereal* pda = pdA + iNumDofs*iNumDofs - 1;
657  if (pdE) {
658  doublereal* pde = pdE + iNumDofs*iNumDofs - 1;
659 
660  for (unsigned int i = iNumDofs; i > 0; i--) {
661  WM.PutRowIndex(i, iFirstIndex + i);
662  WM.PutColIndex(i, iFirstIndex + i);
663  pde -= iNumDofs;
664  pda -= iNumDofs;
665  for (unsigned int j = iNumDofs; j > 0; j--) {
666  /* Attenzione: si assume A orientata per righe:
667  * a_11, a_12, ..., a_1n, a_21, ..., a_2n,
668  * ..., a_nn */
669  WM.PutCoef(i, j, pde[j] - pda[j]*dCoef);
670  }
671  }
672 
673  } else {
674  for (unsigned int i = iNumDofs; i > 0; i--) {
675  WM.PutRowIndex(i, iFirstIndex + i);
676  WM.PutColIndex(i, iFirstIndex + i);
677  pda -= iNumDofs;
678  for (unsigned int j = iNumDofs; j > 0; j--) {
679  /* Attenzione: si assume A orientata per righe:
680  * a_11, a_12, ..., a_1n, a_21, ..., a_2n,
681  * ..., a_nn */
682  WM.PutCoef(i, j, -pda[j]*dCoef);
683  }
684  WM.IncCoef(i, i, 1.);
685  }
686  }
687 
688  return WorkMat;
689 }
690 
691 /* assemblaggio residuo */
694  doublereal /* dCoef */,
695  const VectorHandler& XCurr,
696  const VectorHandler& XPrimeCurr)
697 {
698  DEBUGCOUT("Entering GenelStateSpaceMIMO::AssRes()" << std::endl);
699 
700  integer iNumRows = 0;
701  integer iNumCols = 0;
702  WorkSpaceDim(&iNumRows, &iNumCols);
703  WorkVec.ResizeReset(iNumRows);
704 
705  integer iFirstIndex = iGetFirstIndex();
706 
707  doublereal* pdx = pdX-1;
708  doublereal* pdxp = pdXP-1;
709  for (unsigned int i = iNumDofs; i > 0; i--) {
710  WorkVec.PutRowIndex(i, iFirstIndex+i);
711  pdx[i] = XCurr(iFirstIndex+i);
712  pdxp[i] = XPrimeCurr(iFirstIndex+i);
713  }
714 
715  doublereal* pdc = pdC + iNumOutputs*iNumDofs - 1;
716  if (pdD != 0) {
717  doublereal* pdd = pdD + iNumOutputs*iNumInputs - 1;
718  for (int i = iNumOutputs; i > 0; i--) {
719  integer iRowIndex_y = pvSD_y[i - 1].pNode->iGetFirstRowIndex()+1;
720  WorkVec.PutRowIndex(iNumDofs+i, iRowIndex_y);
721  doublereal y = pvSD_y[i - 1].pNode->dGetX();
722  doublereal d = -y;
723  pdc -= iNumDofs;
724  for (unsigned int j = iNumDofs; j > 0; j--) {
725  d += pdc[j]*pdx[j];
726  }
727  pdd -= iNumInputs;
728  for (unsigned int j = iNumInputs; j > 0; j--) {
729  d += pdd[j]*SV_u[j - 1]->dGetValue();
730  }
731  WorkVec.PutCoef(iNumDofs + i, d);
732  }
733 
734  } else {
735  for (int i = iNumOutputs; i > 0; i--) {
736  integer iRowIndex_y = pvSD_y[i - 1].pNode->iGetFirstRowIndex() + 1;
737  WorkVec.PutRowIndex(iNumDofs + i, iRowIndex_y);
738  doublereal y = pvSD_y[i - 1].pNode->dGetX();
739  doublereal d = -y;
740  pdc -= iNumDofs;
741  for (unsigned int j = iNumDofs; j > 0; j--) {
742  d += pdc[j]*pdx[j];
743  }
744  WorkVec.PutCoef(iNumDofs + i, d);
745  }
746  }
747 
748  doublereal* pda = pdA + iNumDofs*iNumDofs;
749  doublereal* pdb = pdB + iNumDofs*iNumInputs;
750  pdxp = pdXP;
751  pdx = pdX;
752  if (pdE) {
753  doublereal* pde = pdE + iNumDofs*iNumDofs;
754  for (unsigned int i = iNumDofs; i-- > 0; ) {
755  doublereal d = 0.;
756  pdb -= iNumInputs;
757  for (unsigned int j = iNumInputs; j-- > 0; ) {
758  d += pdb[j]*SV_u[j]->dGetValue();
759  }
760  pde -= iNumDofs;
761  pda -= iNumDofs;
762  for (unsigned int j = iNumDofs; j-- > 0; ) {
763  d += pda[j]*pdx[j] - pde[j]*pdxp[j];
764  }
765  WorkVec.PutCoef(i + 1, d);
766  }
767 
768  } else {
769  for (unsigned int i = iNumDofs; i-- > 0; ) {
770  doublereal d = -pdxp[i];
771  pdb -= iNumInputs;
772  for (unsigned int j = iNumInputs; j-- > 0; ) {
773  d += pdb[j]*SV_u[j]->dGetValue();
774  }
775  pda -= iNumDofs;
776  for (unsigned int j = iNumDofs; j-- > 0; ) {
777  d += pda[j]*pdx[j];
778  }
779  WorkVec.PutCoef(i + 1, d);
780  }
781  }
782 
783  return WorkVec;
784 }
785 
786 void
790 {
791  integer iFirstIndex = iGetFirstIndex() + 1;
792 
793  if (pdE == 0) {
794  doublereal* pda = pdA + iNumDofs*iNumDofs;
795  doublereal* pdb = pdB + iNumDofs*iNumInputs;
796 
797  for (unsigned int i = iNumDofs; i-- > 0; ) {
798  pdXP[i] = 0.;
799  pdb -= iNumInputs;
800  for (unsigned int j = iNumInputs; j-- > 0; ) {
801  pdXP[i] += pdb[j]*SV_u[j]->dGetValue();
802  }
803  pda -= iNumDofs;
804  for (unsigned int j = iNumDofs; j-- > 0; ) {
805  pdXP[i] += pda[j]*pdX[j];
806  }
807  }
808  }
809 
810  for (unsigned i = 0; i < iNumDofs; i++) {
811  X(iFirstIndex + i) = pdX[i];
812  XP(iFirstIndex + i) = pdXP[i];
813  }
814 }
815 
816 void
818 {
819  if (bToBeOutput()) {
820  std::ostream& out(OH.Genels());
821  out << std::setw(8) << GetLabel();
822  for (unsigned int i = 0; i < iNumDofs; i++) {
823  out << " " << pdX[i];
824  }
825  for (unsigned int i = 0; i < iNumDofs; i++) {
826  out << " " << pdXP[i];
827  }
828  out << "\n";
829  }
830 }
831 
832 void
834  std::vector<const Node *>& connectedNodes) const {
835  unsigned i, iNodes = iNumOutputs;
836  for (std::vector<ScalarValue *>::const_iterator u = SV_u.begin();
837  u != SV_u.end(); ++u)
838  {
839  if (dynamic_cast<NodeDof *>(*u)) {
840  iNodes++;
841  }
842  }
843 
844  connectedNodes.resize(iNodes);
845  for (i = 0; i < iNumOutputs; i++) {
846  connectedNodes[i] = pvSD_y[i].pNode;
847  }
848 
849  for (std::vector<ScalarValue *>::const_iterator u = SV_u.begin();
850  u != SV_u.end(); ++u)
851  {
852  NodeDof* ndp = dynamic_cast<NodeDof *>(*u);
853  if (ndp) {
854  connectedNodes[i++] = ndp->pNode;
855  }
856  }
857 }
858 
859 /* GenelStateSpaceMIMO - end */
860 
ScalarValue * SV_u
Definition: genfilt.h:45
ScalarNode * pNode
Definition: node.h:708
std::ostream & Genels(void) const
Definition: output.h:513
GenelStateSpaceMIMO(unsigned int uLabel, const DofOwner *pDO, unsigned int iNumOut, const ScalarDof *y, std::vector< ScalarValue * > &u, unsigned int Order, doublereal *pE, doublereal *pA, doublereal *pB, doublereal *pC, doublereal *pD, bool bBalance, doublereal *pdX0, doublereal *pdXP0, flag fOutput)
Definition: genfilt.cc:416
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
doublereal * pdX
Definition: genfilt.h:55
virtual doublereal dGetValue(void) const =0
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genfilt.cc:693
long int flag
Definition: mbdyn.h:43
virtual bool bToBeOutput(void) const
Definition: output.cc:890
doublereal * pdD
Definition: genfilt.h:136
virtual void ResizeReset(integer)
Definition: vh.cc:55
doublereal * pdB
Definition: genfilt.h:134
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
doublereal * pdA
Definition: genfilt.h:133
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: genfilt.cc:787
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &, const VectorHandler &)
Definition: genfilt.cc:599
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &, const VectorHandler &)
Definition: genfilt.cc:204
virtual ~GenelStateSpaceSISO(void)
Definition: genfilt.cc:141
int iOrder
Definition: node.h:710
#define SAFEDELETEARR(pnt)
Definition: mynewmem.h:713
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
unsigned int iNumOutputs
Definition: genfilt.h:125
virtual const doublereal & dGetX(void) const =0
std::vector< Hint * > Hints
Definition: simentity.h:89
void IncCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:683
virtual DofOrder::Order GetDofType(unsigned int i) const
Definition: genfilt.cc:570
virtual std::ostream & Restart(std::ostream &out) const
Definition: genfilt.cc:577
ScalarDof * pvSD_y
Definition: genfilt.h:127
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: genfilt.cc:191
doublereal * pdX
Definition: genfilt.h:138
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
doublereal * pdC
Definition: genfilt.h:135
doublereal dD
Definition: genfilt.h:53
virtual void Output(OutputHandler &OH) const
Definition: genfilt.cc:382
doublereal * pdA
Definition: genfilt.h:50
unsigned int iNumDofs
Definition: genfilt.h:47
#define DEBUGCOUT(msg)
Definition: myassert.h:232
Definition: genel.h:45
virtual integer iGetFirstRowIndex(void) const
Definition: node.cc:82
doublereal * pdXP
Definition: genfilt.h:56
#define ASSERT(expression)
Definition: colamd.c:977
Order
Definition: shapefnc.h:42
ScalarDof SD_y
Definition: genfilt.h:44
virtual void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: genfilt.cc:833
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
virtual void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: genfilt.cc:398
virtual unsigned int iGetNumDof(void) const
Definition: genfilt.cc:564
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: genfilt.cc:583
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual void Output(OutputHandler &OH) const
Definition: genfilt.cc:817
virtual ~GenelStateSpaceMIMO(void)
Definition: genfilt.cc:527
std::vector< ScalarValue * > SV_u
Definition: genfilt.h:128
Definition: elem.h:75
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
doublereal * pdC
Definition: genfilt.h:52
doublereal * pdXP
Definition: genfilt.h:139
doublereal * pdE
Definition: genfilt.h:132
virtual DofOrder::Order GetDofType(unsigned int i) const
Definition: genfilt.cc:176
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
unsigned int iNumInputs
Definition: genfilt.h:126
Definition: node.h:578
Node * pNode
Definition: node.h:582
virtual unsigned int iGetNumDof(void) const
Definition: genfilt.cc:169
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
double doublereal
Definition: colamd.c:52
unsigned int iNumDofs
Definition: genfilt.h:130
long int integer
Definition: colamd.c:51
doublereal * pdE
Definition: genfilt.h:49
void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: genfilt.cc:351
unsigned int GetLabel(void) const
Definition: withlab.cc:62
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genfilt.cc:287
GenelStateSpaceSISO(unsigned int uLabel, const DofOwner *pDO, const ScalarDof &y, ScalarValue *u, unsigned int Order, doublereal *pE, doublereal *pA, doublereal *pB, doublereal *pC, doublereal D, bool bBalance, doublereal *pdX0, doublereal *pdXP0, flag fOutput)
Definition: genfilt.cc:49
virtual std::ostream & Restart(std::ostream &out) const
Definition: genfilt.cc:184
doublereal * pdB
Definition: genfilt.h:51
#define SAFEDELETE(pnt)
Definition: mynewmem.h:710
virtual integer iGetFirstColIndex(void) const
Definition: node.cc:88