1// This file is part of SmallBASIC
2//
3// Math RTL
4//
5// This program is distributed under the terms of the GPL v2.0 or later
6// Download the GNU Public License (GPL) from www.gnu.org
7//
8// Copyright(C) 2000 Nicholas Christopoulos
9
10#include "common/sys.h"
11#include "common/blib_math.h"
12#include "common/kw.h"
13#include "common/var.h"
14#include "common/device.h"
15#include "common/blib.h"
16#include "common/pproc.h"
17#include "common/smbas.h"
18
19/*
20 * INT(x) round downwards to the nearest integer
21 */
22var_num_t fint(var_num_t x) {
23 return (x < 0.0) ? -floor(-x) : floor(x);
24}
25
26/*
27 * FRAC(x)
28 */
29var_num_t frac(var_num_t x) {
30 return fabsl(fabsl(x) - fint(fabsl(x)));
31}
32
33/*
34 * SGN(x)
35 */
36int sgn(var_num_t x) {
37 return (x < 0.0) ? -1 : 1;
38}
39
40/*
41 * ROUND(x, digits)
42 */
43var_num_t fround(var_num_t x, int dig) {
44 var_num_t result;
45 var_num_t m = floor(pow(10.0, dig));
46
47 if (x < 0.0) {
48 result = -floor((-x * m) + .5) / m;
49 } else {
50 result = floor((x * m) + .5) / m;
51 }
52 return result;
53}
54
55void root_iterate(var_num_t xl, var_num_t xh, var_num_t fl,
56 var_num_t fh, var_t *res_vp, var_num_t maxerr,
57 var_t *err_vp, bcip_t use_ip);
58
59/*
60 * length of line
61 */
62var_num_t line_length(var_num_t Ax, var_num_t Ay, var_num_t Bx, var_num_t By) {
63 var_num_t dx, dy;
64
65 dx = Bx - Ax;
66 dy = By - Ay;
67 return sqrt(dx * dx + dy * dy);
68}
69
70/*
71 * Gauss-Jordan method
72 */
73void mat_gauss_jordan(var_num_t *a, var_num_t *b, int n, double toler) {
74 var_num_t big, dum, pivinv, swp;
75 int i, j, k, l, ll;
76 int icol, irow;
77 int *indxc, *indxr, *ipiv;
78
79 //
80 indxc = (int *) malloc(sizeof(int) * n);
81 indxr = (int *) malloc(sizeof(int) * n);
82 ipiv = (int *) malloc(sizeof(int) * n);
83
84 //
85 irow = 0;
86 icol = 0;
87 for (j = 0; j < n; j++) {
88 ipiv[j] = 0;
89 }
90 for (i = 0; i < n; i++) {
91 big = 0.0;
92 for (j = 0; j < n; j++) {
93 if (ipiv[j] != 1) {
94 for (k = 0; k < n; k++) {
95 if (ipiv[k] == 0) {
96 if (ABS(a[j * n + k]) >= big) {
97 big = ABS(a[j * n + k]);
98 irow = j;
99 icol = k;
100 }
101 } else if (ipiv[k] > 1) {
102 err_matsig();
103 free(indxc);
104 free(indxr);
105 free(ipiv);
106 return;
107 }
108 }
109 }
110 }
111
112 ipiv[icol] = ipiv[icol] + 1;
113 if (irow != icol) {
114 for (l = 0; l < n; l++) {
115 SWAP(a[irow * n + l], a[icol * n + l], swp);
116 }
117
118 SWAP(b[irow], b[icol], swp);
119 }
120
121 indxr[i] = irow;
122 indxc[i] = icol;
123
124 if (a[icol * n + icol] < toler) {
125 err_matsig();
126 free(indxc);
127 free(indxr);
128 free(ipiv);
129 return;
130 }
131
132 pivinv = 1.0 / a[icol * n + icol];
133 a[icol * n + icol] = 1.0;
134 for (l = 0; l < n; l++) {
135 a[icol * n + l] = a[icol * n + l] * pivinv;
136 }
137
138 b[icol] = b[icol] * pivinv;
139 for (ll = 0; ll < n; ll++) {
140 if (ll != icol) {
141 dum = a[ll * n + icol];
142 a[ll * n + icol] = 0.0;
143 for (l = 0; l < n; l++) {
144 a[ll * n + l] = a[ll * n + l] - a[icol * n + l] * dum;
145 }
146 b[ll] = b[ll] - b[icol] * dum;
147 }
148 }
149 }
150
151 for (l = n - 1; l >= 0; l--) {
152 if (indxr[l] != indxc[l]) {
153 for (k = 0; k < n; k++) {
154 SWAP(a[k * n + indxr[l]], a[k * n + indxc[l]], swp);
155 }
156 }
157 }
158
159 // ///////////////
160 free(indxc);
161 free(indxr);
162 free(ipiv);
163}
164
165/*
166 */
167void mat_det2(var_num_t t, int m, int k, var_num_t *a, int *done, var_num_t *v, int n, double toler) {
168 if (k >= n) {
169 int s = -1;
170
171 if ((m % 2) == 0) {
172 s = 1;
173 }
174 *v += s * t;
175 } else {
176 if (ABS(t) > toler) {
177 int n2 = 0, j;
178
179 for (j = n - 1; j >= 0; j--) {
180 if (done[j]) {
181 n2++;
182 } else {
183 done[j] = 1;
184 mat_det2(t * a[k * n + j], m + n2, k + 1, a, done, v, n, toler);
185 done[j] = 0;
186 }
187 }
188 }
189 }
190}
191
192/*
193 * Determinant of A
194 */
195var_num_t mat_determ(var_num_t *a, int n, double toler) {
196 int *done;
197 int i;
198 var_num_t v;
199
200 done = malloc(n * sizeof(int));
201 for (i = 0; i < n; i++) {
202 done[i] = 0;
203 }
204 v = 0;
205 mat_det2(1, 0, 0, a, done, &v, n, toler);
206
207 free(done);
208 return v;
209}
210
211/*
212 */
213var_num_t statmeandev(var_num_t *e, int count) {
214 var_num_t sum = 0.0;
215 var_num_t mean;
216 int i;
217
218 if (count == 0) {
219 return 0;
220 }
221
222 for (i = 0; i < count; i++) {
223 sum += e[i];
224 }
225 mean = sum / count;
226
227 sum = 0.0;
228 for (i = 0; i < count; i++) {
229 sum += fabsl(e[i] - mean);
230 }
231
232 return sum / count;
233}
234
235/*
236 */
237var_num_t statspreads(var_num_t *e, int count) {
238 var_num_t sumsq = 0.0, sum = 0.0;
239 int i;
240
241 if (count <= 1) {
242 return 0;
243 }
244
245 for (i = 0; i < count; i++) {
246 sum += e[i];
247 sumsq += (e[i] * e[i]);
248 }
249
250 return sumsq / (count - 1) - (sum * sum) / (count * (count - 1));
251}
252
253/*
254 */
255var_num_t statspreadp(var_num_t *e, int count) {
256 var_num_t sumsq = 0.0, sum = 0.0;
257 int i;
258
259 if (count <= 0) {
260 return 0;
261 }
262
263 for (i = 0; i < count; i++) {
264 sum += e[i];
265 sumsq += (e[i] * e[i]);
266 }
267
268 return sumsq / count - (sum * sum) / (count * count);
269}
270
271//
272// INTEGRAL low, high, maxsegs, maxerr, BYREF result, BYREF err USE f(x)
273//
274
275/*
276 double simpson(double low, double high, int nseg, bcip_t use_ip) SEC(BMATH2);
277 double simpson(double low, double high, int nseg, bcip_t use_ip)
278 {
279 double width, t, sum, x;
280 int m, j;
281 var_t var;
282
283 v_init(&var);
284
285 width = (high - low) / nseg;
286
287 var.type = V_NUM; var.n = low;
288 exec_usefunc(&var, use_ip); if ( prog_error ) return 0;
289 sum = v_getval(&var);
290 v_free(&var);
291
292 var.type = V_NUM; var.n = high;
293 exec_usefunc(&var, use_ip); if ( prog_error ) return 0;
294 sum += v_getval(&var);
295 v_free(&var);
296
297 m = nseg >> 1;
298 t = 0;
299 for ( j = 1; j <= m; j ++ ) {
300 x = low + width * (2 * j - 1);
301
302 var.type = V_NUM; var.n = x;
303 exec_usefunc(&var, use_ip); if ( prog_error ) return 0;
304 t += v_getval(&var);
305 v_free(&var);
306 }
307 sum += 4.0 * t;
308
309 m --;
310 t = 0;
311 for ( j = 1; j <= m; j ++ ) {
312 x = low + width * 2.0 * ((double) j);
313
314 var.type = V_NUM; var.n = x;
315 exec_usefunc(&var, use_ip); if ( prog_error ) return 0;
316 t += v_getval(&var);
317 v_free(&var);
318 }
319
320 sum += 2.0 * t;
321 return width * sum / 3.0;
322 }
323
324 void cmd_integral()
325 {
326 var_t *res_vp, *err_vp;
327 double low, high, maxerr, errval, oldval;
328 int maxseg, nseg;
329 bcip_t use_ip, exit_ip = INVALID_ADDR;
330
331 low = par_getnum(); if ( prog_error ) return;
332 par_getcomma(); if ( prog_error ) return;
333 high = par_getnum(); if ( prog_error ) return;
334 par_getcomma(); if ( prog_error ) return;
335 maxseg = par_getint(); if ( prog_error ) return;
336 par_getcomma(); if ( prog_error ) return;
337 maxerr = par_getnum(); if ( prog_error ) return;
338 par_getcomma(); if ( prog_error ) return;
339 res_vp = par_getvar_ptr(); if ( prog_error ) return;
340 par_getcomma(); if ( prog_error ) return;
341 err_vp = par_getvar_ptr(); if ( prog_error ) return;
342
343 if ( code_peek() != kwUSE ) {
344 rt_raise("INTEGRAL: FUNCTION IS MISSING!");
345 return;
346 }
347
348 // USE
349 code_skipnext();
350 use_ip = code_getaddr();
351 exit_ip = code_getaddr();
352
353 //
354 v_free(res_vp);
355 v_free(err_vp);
356 res_vp->type = V_NUM; res_vp->n = 0;
357 err_vp->type = V_NUM; err_vp->n = 0;
358
359 nseg = 10;
360 oldval = simpson(low, high, nseg, use_ip);
361 if ( !prog_error ) {
362 do {
363 nseg = nseg << 1;
364 res_vp->n = simpson(low, high, nseg, use_ip);
365 if ( prog_error ) break;
366
367 if ( res_vp->n == 0 )
368 errval = 0;
369 else
370 errval = ABS((res_vp->n - oldval) / res_vp->n);
371 if ( nseg > maxseg )
372 err_vp->n = -errval;
373 if ( errval < maxerr )
374 err_vp->n = nseg;
375 oldval = res_vp->n;
376 } while ( err_vp->n == 0.0 );
377 }
378
379 code_jump(exit_ip);
380 }
381 */
382
383//
384// ROOT low, high, maxseg, maxerr, BYREF result, BYREF errcode USE ...
385//
386void root_iterate(var_num_t xl, var_num_t xh, var_num_t fl, var_num_t fh, var_t *res_vp, var_num_t maxerr,
387 var_t *err_vp, bcip_t use_ip) {
388 var_num_t t, x;
389 var_t var;
390
391 v_init(&var);
392 do {
393 x = (xl + xh) / 2.0;
394
395 var.type = V_NUM;
396 var.v.n = x;
397 exec_usefunc(&var, use_ip);
398 if (prog_error) {
399 return;
400 }
401 t = v_getval(&var);
402 v_free(&var);
403
404 if (ABS(t) < maxerr) {
405 res_vp->v.n = x;
406 err_vp->v.i = 0;
407 return;
408 } else {
409 if (t * fl > 0.0) {
410 (xl = x, fl = t);
411 } else {
412 (xh = x, fh = t);
413 }
414 }
415 } while (err_vp->v.i);
416}
417
418void cmd_root() {
419 var_t *res_vp, *err_vp;
420 var_num_t low, high, maxerr;
421 int maxseg, nseg;
422 bcip_t use_ip, exit_ip = INVALID_ADDR;
423 var_t var;
424
425 int j;
426 var_num_t xl, xh, fl, fh, x, width;
427
428 v_init(&var);
429 low = par_getnum();
430 if (prog_error)
431 return;
432 par_getcomma();
433 if (prog_error)
434 return;
435 high = par_getnum();
436 if (prog_error)
437 return;
438 par_getcomma();
439 if (prog_error)
440 return;
441 maxseg = par_getint();
442 if (prog_error)
443 return;
444 par_getcomma();
445 if (prog_error)
446 return;
447 maxerr = par_getnum();
448 if (prog_error)
449 return;
450 par_getcomma();
451 if (prog_error)
452 return;
453 res_vp = par_getvar_ptr();
454 if (prog_error)
455 return;
456 par_getcomma();
457 if (prog_error)
458 return;
459 err_vp = par_getvar_ptr();
460 if (prog_error)
461 return;
462
463 if (code_peek() != kwUSE) {
464 rt_raise("INTEGRAL: FUNCTION IS MISSING!");
465 return;
466 }
467
468 // USE
469 code_skipnext();
470 use_ip = code_getaddr();
471 exit_ip = code_getaddr();
472
473 //
474 v_free(res_vp);
475 v_free(err_vp);
476 res_vp->type = V_NUM;
477 res_vp->v.n = 0;
478 err_vp->type = V_INT;
479 err_vp->v.i = 1;
480
481 xl = low;
482 xh = high;
483
484 var.type = V_NUM;
485 var.v.n = xl;
486 exec_usefunc(&var, use_ip);
487 if (prog_error)
488 return;
489 fl = v_getval(&var);
490 v_free(&var);
491
492 var.type = V_NUM;
493 var.v.n = xh;
494 exec_usefunc(&var, use_ip);
495 if (prog_error)
496 return;
497 fh = v_getval(&var);
498 v_free(&var);
499
500 if (ABS(fl) < maxerr) {
501 err_vp->v.i = 0;
502 res_vp->v.n = xl;
503 code_jump(exit_ip);
504 return;
505 }
506
507 if (ABS(fh) < maxerr) {
508 err_vp->v.i = 0;
509 res_vp->v.n = xh;
510 code_jump(exit_ip);
511 return;
512 }
513
514 if (fl * fh < 0) {
515 root_iterate(xl, xh, fl, fh, res_vp, maxerr, err_vp, use_ip);
516 code_jump(exit_ip);
517 return;
518 }
519
520 nseg = 2;
521 do {
522 width = (xh - xl) / nseg;
523 for (j = 1; j <= (nseg >> 1); j++) {
524 x = xl + width * (2 * j - 1);
525
526 var.type = V_NUM;
527 var.v.n = x;
528 exec_usefunc(&var, use_ip);
529 if (prog_error)
530 return;
531 fh = v_getval(&var);
532 v_free(&var);
533
534 if (fh * fl < 0) {
535 xh = x;
536 root_iterate(xl, xh, fl, fh, res_vp, maxerr, err_vp, use_ip);
537 code_jump(exit_ip);
538 return;
539 }
540 }
541 nseg = nseg << 1;
542 } while (nseg <= maxseg);
543
544 code_jump(exit_ip);
545}
546
547//
548// DERIV x, maxtries, maxerr, BYREF result, BYREF errcode USE ...
549//
550void cmd_deriv() {
551 var_t *res_vp, *err_vp;
552 double maxerr, x;
553 int maxseg, nseg;
554 bcip_t use_ip, exit_ip = INVALID_ADDR;
555 var_t var;
556
557 double delta = 0.01, f1, f2, f3, fp, op = 0.0, errval;
558
559 v_init(&var);
560 x = par_getnum();
561 if (prog_error)
562 return;
563 par_getcomma();
564 if (prog_error)
565 return;
566 maxseg = par_getint();
567 if (prog_error)
568 return;
569 par_getcomma();
570 if (prog_error)
571 return;
572 maxerr = par_getnum();
573 if (prog_error)
574 return;
575 par_getcomma();
576 if (prog_error)
577 return;
578 res_vp = par_getvar_ptr();
579 if (prog_error)
580 return;
581 par_getcomma();
582 if (prog_error)
583 return;
584 err_vp = par_getvar_ptr();
585 if (prog_error)
586 return;
587
588 if (code_peek() != kwUSE) {
589 rt_raise("INTEGRAL: FUNCTION IS MISSING!");
590 return;
591 }
592
593 // USE
594 code_skipnext();
595 use_ip = code_getaddr();
596 exit_ip = code_getaddr();
597
598 //
599 v_free(res_vp);
600 v_free(err_vp);
601 res_vp->type = V_NUM;
602 res_vp->v.n = 0;
603 err_vp->type = V_INT;
604 err_vp->v.i = 1;
605
606 nseg = 0;
607 do {
608 var.type = V_NUM;
609 var.v.n = x;
610 exec_usefunc(&var, use_ip);
611 if (prog_error)
612 return;
613 f1 = v_getval(&var);
614 v_free(&var);
615
616 var.type = V_NUM;
617 var.v.n = x + delta;
618 exec_usefunc(&var, use_ip);
619 if (prog_error)
620 return;
621 f2 = v_getval(&var);
622 v_free(&var);
623
624 var.type = V_NUM;
625 var.v.n = x + 2 * delta;
626 exec_usefunc(&var, use_ip);
627 if (prog_error)
628 return;
629 f3 = v_getval(&var);
630 v_free(&var);
631
632 fp = (-3.0 * f1 + 4.0 * f2 - f3) / 2.0 / delta;
633 if (fp == 0.0)
634 errval = 0;
635 else
636 errval = ABS((fp - op) / fp);
637
638 nseg++;
639 if (nseg >= maxseg) {
640 res_vp->v.n = fp;
641 break;
642 }
643
644 if (errval < maxerr) {
645 res_vp->v.n = fp;
646 err_vp->v.i = 0;
647 }
648
649 delta = delta / 2.0;
650 op = fp;
651
652 } while (err_vp->v.i);
653
654 code_jump(exit_ip);
655}
656
657//
658// DIFFEQ x0, y0, x, maxseg, maxerr, BYREF y, BYREF errcode USE ...
659//
660void cmd_diffeq() {
661 var_t *res_vp, *err_vp;
662 double maxerr;
663 int maxseg, nseg, j;
664 bcip_t use_ip, exit_ip = INVALID_ADDR;
665 var_t var, var2;
666
667 double x0, x1, y0, width, hw, ka, kb, kc, kd, xi, yi;
668 double errval, yp = 0;
669
670 v_init(&var);
671 v_init(&var2);
672 x0 = par_getnum();
673 if (prog_error)
674 return;
675 par_getcomma();
676 if (prog_error)
677 return;
678 y0 = par_getnum();
679 if (prog_error)
680 return;
681 par_getcomma();
682 if (prog_error)
683 return;
684 x1 = par_getnum();
685 if (prog_error)
686 return;
687 par_getcomma();
688 if (prog_error)
689 return;
690 maxseg = par_getint();
691 if (prog_error)
692 return;
693 par_getcomma();
694 if (prog_error)
695 return;
696 maxerr = par_getnum();
697 if (prog_error)
698 return;
699 par_getcomma();
700 if (prog_error)
701 return;
702 res_vp = par_getvar_ptr();
703 if (prog_error)
704 return;
705 par_getcomma();
706 if (prog_error)
707 return;
708 err_vp = par_getvar_ptr();
709 if (prog_error)
710 return;
711
712 if (code_peek() != kwUSE) {
713 rt_raise("INTEGRAL: FUNCTION IS MISSING!");
714 return;
715 }
716
717 // USE
718 code_skipnext();
719 use_ip = code_getaddr();
720 exit_ip = code_getaddr();
721
722 //
723 v_free(res_vp);
724 v_free(err_vp);
725 res_vp->type = V_NUM;
726 res_vp->v.n = 0;
727 err_vp->type = V_INT;
728 err_vp->v.i = 1;
729
730 nseg = 1;
731 do {
732 width = (x1 - x0) / nseg;
733 hw = width / 2.0;
734
735 for (j = 1; j <= nseg; j++) {
736 xi = x0 + (j - 1) * width;
737 // xf = xi + width;
738 if (j == 1)
739 yi = y0;
740 else
741 yi = res_vp->v.n;
742
743 var.type = V_NUM;
744 var.v.n = xi;
745 var2.type = V_NUM;
746 var2.v.n = yi;
747 exec_usefunc2(&var, &var2, use_ip);
748 if (prog_error)
749 return;
750 ka = v_getval(&var);
751 v_free(&var);
752
753 var.type = V_NUM;
754 var.v.n = xi + hw;
755 var2.type = V_NUM;
756 var2.v.n = yi + ka * hw;
757 exec_usefunc2(&var, &var2, use_ip);
758 if (prog_error)
759 return;
760 kb = v_getval(&var);
761 v_free(&var);
762
763 var.type = V_NUM;
764 var.v.n = xi + hw;
765 var2.type = V_NUM;
766 var2.v.n = yi + kb * hw;
767 exec_usefunc2(&var, &var2, use_ip);
768 if (prog_error)
769 return;
770 kc = v_getval(&var);
771 v_free(&var);
772
773 var.type = V_NUM;
774 var.v.n = xi + width;
775 var2.type = V_NUM;
776 var2.v.n = yi + kc * width;
777 exec_usefunc2(&var, &var2, use_ip);
778 if (prog_error)
779 return;
780 kd = v_getval(&var);
781 v_free(&var);
782
783 res_vp->v.n = yi + width * (ka + 2.0 * kb + 2.0 * kc + kd) / 6.0;
784 }
785
786 if (nseg == 1)
787 errval = maxerr;
788 else {
789 if (res_vp->v.n == 0)
790 errval = 0;
791 else
792 errval = ABS((res_vp->v.n - yp) / res_vp->v.n);
793 }
794
795 if (nseg > maxseg)
796 break;
797
798 if (errval < maxerr || width == 0) {
799 err_vp->v.i = 0;
800 break;
801 }
802
803 yp = res_vp->v.n;
804 nseg = nseg << 1;
805 } while (err_vp->v.i);
806
807 code_jump(exit_ip);
808}
809