1 /* Scicos
2 *
3 * Copyright (C) INRIA - METALAU Project <scicos@inria.fr>
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 *
19 * See the file ./license.txt
20 */
21 /*--------------------------------------------------------------------------*/
22 #include <string.h>
23 #include "sci_malloc.h"
24 #include "machine.h"
25 #include "scicos_block.h"
26 #include "dynlib_scicos_blocks.h"
27 #include "scicos_free.h"
28 #include "scicos_malloc.h"
29 /*--------------------------------------------------------------------------*/
30 extern int C2F(dmmul)(double *a, int *na, double *b, int *nb, double *c__,
31 int *nc, int *l, int *m, int *n);
32 extern int C2F(dmmul1)(double *a, int *na, double *b, int *nb, double *c__,
33 int *nc, int *l, int *m, int *n);
34 /*--------------------------------------------------------------------------*/
dsslti4(scicos_block * block,int flag)35 SCICOS_BLOCKS_IMPEXP void dsslti4(scicos_block *block, int flag)
36 {
37 /* Copyright INRIA
38
39 Scicos block simulator
40 discrete state space linear system simulator
41 rpar(1:nx*nx)=A
42 rpar(nx*nx+1:nx*nx+nx*nu)=B
43 rpar(nx*nx+nx*nu+1:nx*nx+nx*nu+nx*ny)=C */
44
45 int un = 1, lb = 0, lc = 0, ld = 0;
46 int nz = block->nz;
47 int nout = block->nout;
48 int nin = block->nin;
49 double* z = NULL;
50 double* rpar = block->rpar;
51 double* y = NULL;
52 double* u = NULL;
53 int empty[2] = {0, 0};
54 int* outsz = empty;
55 int* insz = empty;
56 double *w = NULL;
57
58 if (nout > 0)
59 {
60 outsz = block->outsz;
61 y = (double*) block->outptr[0];
62 }
63 if (nin > 0)
64 {
65 insz = block->insz;
66 u = (double *) block->inptr[0];
67 }
68 if (nz > 0)
69 {
70 z = block->z;
71 }
72
73 lb = nz * nz;
74
75 if (flag == 1 || flag == 6)
76 {
77 /* y=c*x+d*u */
78 if (nout > 0)
79 {
80 lc = lb + nz * insz[0];
81 ld = lc + nz * outsz[0];
82 if (nz == 0)
83 {
84 if (nin > 0)
85 {
86 C2F(dmmul)(&rpar[ld], outsz, u, insz, y, outsz, outsz, insz, &un);
87 }
88 }
89 else
90 {
91 C2F(dmmul)(&rpar[lc], outsz, z, &nz, y, outsz, outsz, &nz, &un);
92 if (nin > 0)
93 {
94 C2F(dmmul1)(&rpar[ld], outsz, u, insz, y, outsz, outsz, insz, &un);
95 }
96 }
97 }
98 }
99 else if (flag == 2)
100 {
101 /* x+=a*x+b*u */
102 if (nz > 0)
103 {
104 w = (double*) *block->work;
105 memcpy(w, z, nz * sizeof(double));
106 C2F(dmmul)(&rpar[0], &nz, w, &nz, z, &nz, &nz, &nz, &un);
107 if (nin > 0)
108 {
109 C2F(dmmul1)(&rpar[lb], &nz, u, insz, z, &nz, &nz, insz, &un);
110 }
111 }
112 }
113 else if (flag == 4)
114 {
115 /* the workspace for temp storage */
116 if (nz > 0)
117 {
118 *block->work = scicos_malloc(sizeof(double) * nz);
119 if (*block->work == NULL)
120 {
121 set_block_error(-16);
122 return;
123 }
124 }
125 }
126 else if (flag == 5)
127 {
128 if (nz > 0)
129 {
130 scicos_free(*block->work);
131 }
132 }
133 }
134 /*--------------------------------------------------------------------------*/
135
136