1 /*
2 * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 * Copyright (C) 2014 - Scilab Enterprises - Cedric DELAMARRE
4 *
5 * Copyright (C) 2012 - 2016 - Scilab Enterprises
6 *
7 * This file is hereby licensed under the terms of the GNU GPL v2.0,
8 * pursuant to article 5.3.4 of the CeCILL v.2.1.
9 * This file was originally licensed under the terms of the CeCILL v2.1,
10 * and continues to be available under such terms.
11 * For more information, see the COPYING file which you should have received
12 * along with this program.
13 *
14 */
15 /*--------------------------------------------------------------------------*/
16 #include "cacsd_gw.hxx"
17 #include "function.hxx"
18 #include "double.hxx"
19 #include "polynom.hxx"
20 #include "numericconstants.hxx"
21
22 extern "C"
23 {
24 #include "sciprint.h"
25 #include "Scierror.h"
26 #include "localization.h"
27 #include "elem_common.h"
28
29 extern void C2F(dfrmg)( int*, int*, int*, int*, int*, int*, int*,
30 double*, double*, double*, double*, double*,
31 double*, double*, double*, double*, int*);
32
33 extern void C2F(dadd)(int*, double*, int*, double*, int*);
34 extern void C2F(horner)(double*, int*, double*, double*, double*, double*);
35 }
36 /*--------------------------------------------------------------------------*/
37 types::Function::ReturnValue freqRational(types::typed_list &in, int _iRetCount, types::typed_list &out);
38 types::Function::ReturnValue freqState(types::typed_list &in, int _iRetCount, types::typed_list &out);
39 /*--------------------------------------------------------------------------*/
sci_freq(types::typed_list & in,int _iRetCount,types::typed_list & out)40 types::Function::ReturnValue sci_freq(types::typed_list &in, int _iRetCount, types::typed_list &out)
41 {
42 if (in.size() < 3 || in.size() > 5)
43 {
44 Scierror(77, _("%s: Wrong number of input argument(s): %d to %d expected.\n"), "freq", 3, 5);
45 return types::Function::Error;
46 }
47
48 if (_iRetCount > 1)
49 {
50 Scierror(78, _("%s: Wrong number of output argument(s): %d expected.\n"), "freq", 1);
51 return types::Function::Error;
52 }
53
54 if (in.size() == 3) // systeme defini par sa representation rationnelle n./d
55 {
56 return freqRational(in, _iRetCount, out);
57 }
58 else // systeme defini par sa representation d'etat
59 {
60 return freqState(in, _iRetCount, out);
61 }
62 }
63
freqRational(types::typed_list & in,int _iRetCount,types::typed_list & out)64 types::Function::ReturnValue freqRational(types::typed_list &in, int _iRetCount, types::typed_list &out)
65 {
66 int iRowNum = 0;
67 int iColNum = 0;
68 int iRowDen = 0;
69 int iColDen = 0;
70 int iSizeF = 0;
71 int iOne = 1;
72 int iComplex = 0;
73 int iError = 0;
74 double dZero = 0;
75
76 double** pdblDen = NULL;
77 double** pdblNum = NULL;
78 double* pdblF = NULL;
79 double* pdblFImg = NULL;
80 double* pdblR = NULL;
81 double* pdblI = NULL;
82 int* piRankDen = NULL;
83 int* piRankNum = NULL;
84
85 /*** get inputs arguments ***/
86 // get f
87 if (in[2]->isDouble() == false)
88 {
89 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 3);
90 return types::Function::Error;
91 }
92
93 types::Double* pDblF = in[2]->getAs<types::Double>();
94 iSizeF = pDblF->getSize();
95 pdblF = pDblF->get();
96
97 if (pDblF->isComplex())
98 {
99 pdblFImg = pDblF->getImg();
100 iComplex = 1;
101 }
102 else
103 {
104 pdblFImg = &dZero;
105 }
106
107 try
108 {
109 // get DEN
110 if (in[1]->isDouble())
111 {
112 types::Double* pDblDen = in[1]->getAs<types::Double>();
113 double* pdbl = pDblDen->get();
114 if (pDblDen->isComplex())
115 {
116 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 2);
117 return types::Function::Error;
118 }
119
120 iRowDen = pDblDen->getRows();
121 iColDen = pDblDen->getCols();
122
123 piRankDen = new int[pDblDen->getSize()];
124 memset(piRankDen, 0x00, pDblDen->getSize() * sizeof(int));
125
126 pdblDen = new double*[pDblDen->getSize()];
127 for (int i = 0; i < pDblDen->getSize(); i++)
128 {
129 pdblDen[i] = pdbl + i;
130 }
131 }
132 else if (in[1]->isPoly())
133 {
134 types::Polynom* pPolyDen = in[1]->getAs<types::Polynom>();
135
136 double dblEps = NumericConstants::eps;
137
138 if (pPolyDen->isComplex())
139 {
140 bool cplx = false;
141
142 int iSize = pPolyDen->getSize();
143 for (int i = 0; i < iSize; i++)
144 {
145 types::SinglePoly *sp = pPolyDen->get(i);
146 double *df = sp->getImg();
147
148 for (int j = 0 ; j < sp->getSize(); j++)
149 {
150 if (abs(df[j]) > dblEps)
151 {
152 cplx = true;
153
154 break;
155 }
156 }
157 }
158
159 if (cplx)
160 {
161
162 Scierror(999, _("%s: Wrong type for input argument #%d: A real polynom expected.\n"), "freq", 2);
163 return types::Function::Error;
164 }
165
166 }
167
168 iRowDen = pPolyDen->getRows();
169 iColDen = pPolyDen->getCols();
170
171 piRankDen = new int[pPolyDen->getSize()];
172 pPolyDen->getRank(piRankDen);
173
174 pdblDen = new double*[pPolyDen->getSize()];
175 for (int i = 0; i < pPolyDen->getSize(); i++)
176 {
177 pdblDen[i] = pPolyDen->get(i)->get();
178 }
179 }
180 else
181 {
182 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix or polynom expected.\n"), "freq", 2);
183 return types::Function::Error;
184 }
185
186 // get NUM
187 if (in[0]->isDouble())
188 {
189 types::Double* pDblNum = in[0]->getAs<types::Double>();
190 double* pdbl = pDblNum->get();
191 if (pDblNum->isComplex())
192 {
193 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 1);
194 throw 1;
195 }
196
197 iRowNum = pDblNum->getRows();
198 iColNum = pDblNum->getCols();
199
200 piRankNum = new int[pDblNum->getSize()];
201 memset(piRankNum, 0x00, pDblNum->getSize() * sizeof(int));
202
203 pdblNum = new double*[pDblNum->getSize()];
204 for (int i = 0; i < pDblNum->getSize(); i++)
205 {
206 pdblNum[i] = pdbl + i;
207 }
208 }
209 else if (in[0]->isPoly())
210 {
211 types::Polynom* pPolyNum = in[0]->getAs<types::Polynom>();
212
213 double dblEps = NumericConstants::eps;
214 if (pPolyNum->isComplex())
215 {
216 bool cplx = false;
217
218 int iSize = pPolyNum->getSize();
219 for (int i = 0; i < iSize; i++)
220 {
221 types::SinglePoly *sp = pPolyNum->get(i);
222 double *df = sp->getImg();
223
224 for (int j = 0 ; j < sp->getSize(); j++)
225 {
226 if (abs(df[j]) > dblEps)
227 {
228 cplx = true;
229
230 break;
231 }
232 }
233 }
234
235 if (cplx)
236 {
237
238 Scierror(999, _("%s: Wrong type for input argument #%d: A real polynom expected.\n"), "freq", 1);
239 delete[] piRankDen;
240 delete[] pdblDen;
241 return types::Function::Error;
242 }
243 }
244 iRowNum = pPolyNum->getRows();
245 iColNum = pPolyNum->getCols();
246
247 piRankNum = new int[pPolyNum->getSize()];
248 pPolyNum->getRank(piRankNum);
249
250 pdblNum = new double*[pPolyNum->getSize()];
251 for (int i = 0; i < pPolyNum->getSize(); i++)
252 {
253 pdblNum[i] = pPolyNum->get(i)->get();
254 }
255 }
256 else
257 {
258 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix or polynom expected.\n"), "freq", 1);
259 delete[] piRankDen;
260 delete[] pdblDen;
261 return types::Function::Error;
262 }
263
264 if (iRowNum != iRowDen || iColNum != iColDen)
265 {
266 Scierror(60, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "freq");
267 throw 1;
268 }
269
270 /*** perform operations ***/
271 double dVr = 0;
272 double dVi = 0;
273 double dUr = 0;
274 double dUi = 0;
275 int iSize = iRowDen * iColDen * iSizeF;
276
277 pdblR = new double[iSize];
278 pdblI = new double[iSize];
279
280 double* pdblRTemp = pdblR;
281 double* pdblITemp = pdblI;
282
283 for (int i = 0; i < iSizeF; i++)
284 {
285 for (int j = 0; j < iRowDen * iColDen; j++)
286 {
287 C2F(horner)(pdblNum[j], piRankNum + j, pdblF, pdblFImg, &dVr, &dVi);
288 C2F(horner)(pdblDen[j], piRankDen + j, pdblF, pdblFImg, &dUr, &dUi);
289 if (dUr * dUr + dUi * dUi == 0)
290 {
291 Scierror(27, _("%s: Division by zero...\n"), "freq");
292 throw 1;
293 }
294
295 if (iComplex)
296 {
297 C2F(wdiv)(&dVr, &dVi, &dUr, &dUi, pdblRTemp, pdblITemp);
298 }
299 else
300 {
301 *pdblRTemp = dVr / dUr;
302 }
303
304 pdblRTemp++;
305 pdblITemp++;
306 }
307
308 pdblF++;
309 pdblFImg += iComplex;
310 }
311
312 /*** retrun output arguments ***/
313 types::Double* pDblOut = new types::Double(iRowDen, iColDen * iSizeF, iComplex == 1);
314 double* pdblOut = pDblOut->get();
315 int iSizeOut = pDblOut->getSize();
316 C2F(dcopy)(&iSizeOut, pdblR, &iOne, pdblOut, &iOne);
317
318 if (iComplex)
319 {
320 double* pdblOutImg = pDblOut->getImg();
321 C2F(dcopy)(&iSizeOut, pdblI, &iOne, pdblOutImg, &iOne);
322 }
323
324 out.push_back(pDblOut);
325 }
326 catch (int iErr)
327 {
328 iError = iErr;
329 }
330
331 // free memory
332 delete[] piRankDen;
333 delete[] pdblDen;
334
335 if (pdblR)
336 {
337 delete[] pdblR;
338 }
339 if (pdblI)
340 {
341 delete[] pdblI;
342 }
343
344 if (piRankNum)
345 {
346 delete[] piRankNum;
347 }
348 if (pdblNum)
349 {
350 delete[] pdblNum;
351 }
352
353 if (iError)
354 {
355 return types::Function::Error;
356 }
357
358 return types::Function::OK;
359 }
360
freqState(types::typed_list & in,int _iRetCount,types::typed_list & out)361 types::Function::ReturnValue freqState(types::typed_list &in, int _iRetCount, types::typed_list &out)
362 {
363 types::Double* pDblA = NULL;
364 types::Double* pDblB = NULL;
365 types::Double* pDblC = NULL;
366 types::Double* pDblD = NULL;
367 types::Double* pDblF = NULL;
368
369 double dZero = 0;
370
371 int iRowsA = 0;
372 int iColsB = 0;
373 int iRowsC = 0;
374 int iSizeF = 0;
375 int iOne = 1;
376 int iComplex = 0;
377 int iSizeD = 0;
378 int iSizeC = 0;
379 int iSizeB = 0;
380 int iSizeA = 0;
381
382
383 double* pdblA = NULL;
384 double* pdblB = NULL;
385 double* pdblC = NULL;
386 double* pdblD = NULL;
387 double* pdblF = NULL;
388 double* pdblFImg = NULL;
389
390 /*** get inputs arguments ***/
391 int iNbInputArg = (int)in.size();
392 // get f
393 if (in[iNbInputArg - 1]->isDouble() == false)
394 {
395 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", iNbInputArg);
396 return types::Function::Error;
397 }
398
399 pDblF = in[iNbInputArg - 1]->getAs<types::Double>();
400 pdblF = pDblF->get();
401 if (pDblF->isComplex())
402 {
403 pdblFImg = pDblF->getImg();
404 iComplex = 1;
405 }
406 else
407 {
408 pdblFImg = &dZero;
409 }
410
411
412 if (iNbInputArg == 5)
413 {
414 //get D
415 if (in[3]->isDouble() == false)
416 {
417 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 4);
418 return types::Function::Error;
419 }
420
421 pDblD = in[3]->getAs<types::Double>();
422 if (pDblD->isComplex())
423 {
424 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 4);
425 return types::Function::Error;
426 }
427 }
428
429 // get C
430 if (in[2]->isDouble() == false)
431 {
432 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 3);
433 return types::Function::Error;
434 }
435
436 pDblC = in[2]->getAs<types::Double>();
437
438 if (pDblC->isComplex())
439 {
440 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 3);
441 return types::Function::Error;
442 }
443
444 // get B
445 if (in[1]->isDouble() == false)
446 {
447 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 2);
448 return types::Function::Error;
449 }
450
451 pDblB = in[1]->getAs<types::Double>();
452
453 if (pDblB->isComplex())
454 {
455 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 2);
456 return types::Function::Error;
457 }
458
459 // get A
460 if (in[0]->isDouble() == false)
461 {
462 Scierror(999, _("%s: Wrong type for input argument #%d: A matrix expected.\n"), "freq", 1);
463 return types::Function::Error;
464 }
465
466 pDblA = in[0]->getAs<types::Double>();
467
468 if (pDblA->isComplex())
469 {
470 Scierror(999, _("%s: Wrong type for input argument #%d: A real matrix expected.\n"), "freq", 1);
471 return types::Function::Error;
472 }
473
474 if (pDblA->getRows() != pDblA->getCols())
475 {
476 Scierror(999, _("%s: Wrong size for input argument #%d: A square matrix expected.\n"), "freq", 1);
477 return types::Function::Error;
478 }
479
480 iRowsA = pDblA->getRows();
481 iColsB = pDblB->getCols();
482 iRowsC = pDblC->getRows();
483 iSizeF = pDblF->getSize();
484
485 if (iRowsA != pDblB->getRows() || iRowsA != pDblC->getCols())
486 {
487 Scierror(999, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "ppol");
488 return types::Function::Error;
489 }
490
491 if (iNbInputArg == 5 && (pDblD->getRows() != pDblC->getRows() || pDblD->getCols() != pDblB->getCols()))
492 {
493 Scierror(999, _("%s: Wrong size for argument: Incompatible dimensions.\n"), "ppol");
494 return types::Function::Error;
495 }
496
497 /*** perform operations ***/
498 int iJob = 0;
499 bool bFirst = true;
500 double dRcond = 0;
501
502 int* pdblW1 = new int[iRowsA];
503 double* pdblW = new double[2 * iRowsA * iRowsA + 2 * iRowsA];
504 double* pdblWgr = new double[iColsB * iSizeF * iRowsC];
505 double* pdblWgi = new double[iColsB * iSizeF * iRowsC];
506
507 if (iNbInputArg == 5)
508 {
509 iSizeD = pDblD->getSize();
510 pdblD = new double[iSizeD];
511 memcpy(pdblD, pDblD->get(), iSizeD * sizeof(double));
512 }
513
514 iSizeC = pDblC->getSize();
515 pdblC = new double[iSizeC];
516 memcpy(pdblC, pDblC->get(), iSizeC * sizeof(double));
517 iSizeB = pDblB->getSize();
518 pdblB = new double[iSizeB];
519 memcpy(pdblB, pDblB->get(), iSizeB * sizeof(double));
520 iSizeA = pDblA->getSize();
521 pdblA = new double[iSizeA];
522 memcpy(pdblA, pDblA->get(), iSizeA * sizeof(double));
523
524 for (int i = 0; i < iSizeF; i++)
525 {
526 int ig = i * iColsB * iRowsC;
527 C2F(dfrmg)( &iJob, &iRowsA, &iRowsA, &iRowsC, &iRowsC, &iColsB, &iRowsA,
528 pdblA, pdblB, pdblC, pdblF, pdblFImg, pdblWgr + ig, pdblWgi + ig, &dRcond, pdblW, pdblW1);
529
530 if (bFirst && dRcond + 1 == 1)
531 {
532 sciprint(_("Warning :\n"));
533 sciprint(_("matrix is close to singular or badly scaled. rcond = %g\n"), dRcond);
534 bFirst = false;
535 }
536
537 if (iNbInputArg == 5)
538 {
539 int iSize = iColsB * iRowsC;
540 C2F(dadd)(&iSize, pdblD, &iOne, pdblWgr + ig, &iOne);
541 }
542
543 pdblF++;
544 pdblFImg += iComplex;
545 }
546
547 delete[] pdblA;
548 delete[] pdblB;
549 delete[] pdblC;
550
551 if (iNbInputArg == 5)
552 {
553 delete[] pdblD;
554 }
555
556 /*** retrun output arguments ***/
557 types::Double* pDblOut = new types::Double(iRowsC, iColsB * iSizeF, iComplex == 1);
558 double* pdblOutReal = pDblOut->get();
559 double* pdblOutImg = pDblOut->getImg();
560 int iSizeOut = pDblOut->getSize();
561
562 C2F(dcopy)(&iSizeOut, pdblWgr, &iOne, pdblOutReal, &iOne);
563 if (iComplex)
564 {
565 C2F(dcopy)(&iSizeOut, pdblWgi, &iOne, pdblOutImg, &iOne);
566 }
567
568 // free memory
569 delete[] pdblW;
570 delete[] pdblW1;
571 delete[] pdblWgr;
572 delete[] pdblWgi;
573
574 out.push_back(pDblOut);
575 return types::Function::OK;
576 }
577 /*--------------------------------------------------------------------------*/
578