Subversion Repositories Kolibri OS

Rev

Rev 7983 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 7983 Rev 8097
Line 1... Line 1...
1
(*
1
(*
2
    BSD 2-Clause License
2
    BSD 2-Clause License
Line 3... Line 3...
3
 
3
 
4
    Copyright (c) 2019, Anton Krotov
4
    Copyright (c) 2019-2020, Anton Krotov
5
    All rights reserved.
5
    All rights reserved.
Line 6... Line 6...
6
*)
6
*)
Line 7... Line 7...
7
 
7
 
Line 8... Line 8...
8
MODULE Math;
8
MODULE Math;
Line 9... Line -...
9
 
-
 
10
IMPORT SYSTEM;
9
 
11
 
10
IMPORT SYSTEM;
Line -... Line 11...
-
 
11
 
12
 
12
 
-
 
13
CONST
-
 
14
 
-
 
15
    pi* = 3.1415926535897932384626433832795028841972E0;
-
 
16
    e*  = 2.7182818284590452353602874713526624977572E0;
-
 
17
 
13
CONST
18
    ZERO      = 0.0E0;
-
 
19
    ONE       = 1.0E0;
-
 
20
    HALF      = 0.5E0;
-
 
21
    TWO       = 2.0E0;
-
 
22
    sqrtHalf  = 0.70710678118654752440E0;
-
 
23
    eps       = 5.5511151E-17;
Line 14... Line 24...
14
 
24
    ln2Inv    = 1.44269504088896340735992468100189213E0;
Line 15... Line 25...
15
    e   *= 2.71828182845904523;
25
    piInv     = ONE / pi;
Line 16... Line 26...
16
    pi  *= 3.14159265358979324;
26
    Limit     = 1.0536712E-8;
17
    ln2 *= 0.693147180559945309;
27
    piByTwo   = pi / TWO;
18
 
28
 
19
    eps = 1.0E-16;
29
    expoMax   = 1023;
20
    MaxCosArg = 1000000.0 * pi;
30
    expoMin   = 1 - expoMax;
21
 
31
 
22
 
32
 
23
VAR
33
VAR
Line 24... Line 34...
24
 
34
 
25
    Exp: ARRAY 710 OF REAL;
35
    LnInfinity, LnSmall, large, miny: REAL;
Line -... Line 36...
-
 
36
 
-
 
37
 
-
 
38
PROCEDURE [stdcall64] sqrt* (x: REAL): REAL;
-
 
39
BEGIN
-
 
40
    ASSERT(x >= ZERO);
-
 
41
    SYSTEM.CODE(
-
 
42
    0F2H, 0FH, 51H, 45H, 10H,  (*  sqrtsd  xmm0, qword[rbp + 10h]  *)
-
 
43
    05DH,                      (*  pop     rbp                     *)
-
 
44
    0C2H, 08H, 00H             (*  ret     8                       *)
-
 
45
    )
26
 
46
 
27
 
47
    RETURN 0.0
-
 
48
END sqrt;
28
PROCEDURE [stdcall64] sqrt* (x: REAL): REAL;
49
 
-
 
50
 
-
 
51
PROCEDURE sqri* (x: INTEGER): INTEGER;
-
 
52
    RETURN x * x
-
 
53
END sqri;
-
 
54
 
Line 29... Line 55...
29
BEGIN
55
 
30
    ASSERT(x >= 0.0);
56
PROCEDURE sqrr* (x: REAL): REAL;
31
    SYSTEM.CODE(
-
 
32
    0F2H, 0FH, 51H, 45H, 10H,  (*  sqrtsd  xmm0, qword[rbp + 10h]  *)
57
    RETURN x * x
Line 33... Line 58...
33
    05DH,                      (*  pop     rbp                     *)
58
END sqrr;
34
    0C2H, 08H, 00H             (*  ret     8                       *)
-
 
35
    )
59
 
36
 
60
 
37
    RETURN 0.0
-
 
38
END sqrt;
-
 
39
 
61
PROCEDURE exp* (x: REAL): REAL;
40
 
62
CONST
41
PROCEDURE exp* (x: REAL): REAL;
63
    c1 =  0.693359375E0;
42
CONST
64
    c2 = -2.1219444005469058277E-4;
-
 
65
    P0 =  0.249999999999999993E+0;
43
    e25 = 1.284025416687741484; (* exp(0.25) *)
66
    P1 =  0.694360001511792852E-2;
44
 
67
    P2 =  0.165203300268279130E-4;
45
VAR
-
 
46
    a, s, res: REAL;
68
    Q1 =  0.555538666969001188E-1;
47
    neg: BOOLEAN;
69
    Q2 =  0.495862884905441294E-3;
48
    n: INTEGER;
-
 
49
 
70
 
Line 50... Line -...
50
BEGIN
-
 
51
    neg := x < 0.0;
-
 
52
    IF neg THEN
-
 
53
        x := -x
-
 
54
    END;
-
 
55
 
71
VAR
56
    IF x < FLT(LEN(Exp)) THEN
72
    xn, g, p, q, z: REAL;
57
        res := Exp[FLOOR(x)];
73
    n: INTEGER;
58
        x := x - FLT(FLOOR(x));
74
 
59
        WHILE x >= 0.25 DO
-
 
60
            res := res * e25;
75
BEGIN
61
            x := x - 0.25
76
    IF x > LnInfinity THEN
62
        END
-
 
63
    ELSE
77
        x := SYSTEM.INF()
64
        res := SYSTEM.INF();
78
    ELSIF x < LnSmall THEN
Line 65... Line 79...
65
        x := 0.0
79
        x := ZERO
66
    END;
80
    ELSIF ABS(x) < eps THEN
Line 67... Line 81...
67
 
81
        x := ONE
-
 
82
    ELSE
-
 
83
        IF x >= ZERO THEN
-
 
84
            n := FLOOR(ln2Inv * x + HALF)
-
 
85
        ELSE
-
 
86
            n := FLOOR(ln2Inv * x - HALF)
-
 
87
        END;
-
 
88
 
-
 
89
        xn := FLT(n);
-
 
90
        g  := (x - xn * c1) - xn * c2;
-
 
91
        z  := g * g;
68
    n := 0;
92
        p  := ((P2 * z + P1) * z + P0) * g;
69
    a := 1.0;
93
        q  := (Q2 * z + Q1) * z + HALF;
70
    s := 1.0;
94
        x  := HALF + p / (q - p);
Line 71... Line 95...
71
 
95
        PACK(x, n + 1)
72
    REPEAT
96
    END
-
 
97
 
73
        INC(n);
98
    RETURN x
-
 
99
END exp;
Line -... Line 100...
-
 
100
 
-
 
101
 
74
        a := a * x / FLT(n);
102
PROCEDURE ln* (x: REAL): REAL;
-
 
103
CONST
-
 
104
    c1 =  355.0E0 / 512.0E0;
75
        s := s + a
105
    c2 = -2.121944400546905827679E-4;
76
    UNTIL a < eps;
106
    P0 = -0.64124943423745581147E+2;
77
 
107
    P1 =  0.16383943563021534222E+2;
78
    IF neg THEN
108
    P2 = -0.78956112887491257267E+0;
79
        res := 1.0 / (res * s)
109
    Q0 = -0.76949932108494879777E+3;
80
    ELSE
110
    Q1 =  0.31203222091924532844E+3;
81
        res := res * s
111
    Q2 = -0.35667977739034646171E+2;
82
    END
112
 
83
 
113
VAR
84
    RETURN res
114
    zn, zd, r, z, w, p, q, xn: REAL;
Line 85... Line 115...
85
END exp;
115
    n: INTEGER;
86
 
116
 
Line 87... Line 117...
87
 
117
BEGIN
88
PROCEDURE ln* (x: REAL): REAL;
118
    ASSERT(x > ZERO);
89
VAR
119
 
90
    a, x2, res: REAL;
120
    UNPK(x, n);
91
    n: INTEGER;
121
    x := x * HALF;
Line -... Line 122...
-
 
122
 
-
 
123
    IF x > sqrtHalf THEN
-
 
124
        zn := x - ONE;
-
 
125
        zd := x * HALF + HALF;
-
 
126
        INC(n)
-
 
127
    ELSE
-
 
128
        zn := x - HALF;
-
 
129
        zd := zn * HALF + HALF
-
 
130
    END;
-
 
131
 
-
 
132
    z  := zn / zd;
-
 
133
    w  := z * z;
-
 
134
    q  := ((w + Q2) * w + Q1) * w + Q0;
-
 
135
    p  := w * ((P2 * w + P1) * w + P0);
-
 
136
    r  := z + z * (p / q);
-
 
137
    xn := FLT(n)
-
 
138
 
-
 
139
    RETURN (xn * c2 + r) + xn * c1
-
 
140
END ln;
-
 
141
 
-
 
142
 
-
 
143
PROCEDURE power* (base, exponent: REAL): REAL;
-
 
144
BEGIN
-
 
145
    ASSERT(base > ZERO)
-
 
146
    RETURN exp(exponent * ln(base))
-
 
147
END power;
-
 
148
 
-
 
149
 
-
 
150
PROCEDURE ipower* (base: REAL; exponent: INTEGER): REAL;
-
 
151
VAR
-
 
152
    i: INTEGER;
-
 
153
    a: REAL;
-
 
154
 
-
 
155
BEGIN
92
 
156
    a := 1.0;
93
BEGIN
157
 
94
    ASSERT(x > 0.0);
158
    IF base # 0.0 THEN
95
    UNPK(x, n);
159
        IF exponent # 0 THEN
96
 
160
            IF exponent < 0 THEN
97
    x   := (x - 1.0) / (x + 1.0);
161
                base := 1.0 / base
Line 98... Line 162...
98
    x2  := x * x;
162
            END;
-
 
163
            i := ABS(exponent);
-
 
164
            WHILE i > 0 DO
-
 
165
                WHILE ~ODD(i) DO
-
 
166
                    i := LSR(i, 1);
-
 
167
                    base := sqrr(base)
-
 
168
                END;
-
 
169
                DEC(i);
-
 
170
                a := a * base
-
 
171
            END
-
 
172
        ELSE
-
 
173
            a := 1.0
-
 
174
        END
-
 
175
    ELSE
99
    res := x + FLT(n) * (ln2 * 0.5);
176
        ASSERT(exponent > 0);
100
    n   := 1;
-
 
101
 
177
        a := 0.0
-
 
178
    END
Line 102... Line 179...
102
    REPEAT
179
 
-
 
180
    RETURN a
-
 
181
END ipower;
-
 
182
 
-
 
183
 
-
 
184
PROCEDURE log* (base, x: REAL): REAL;
-
 
185
BEGIN
-
 
186
    ASSERT(base > ZERO);
103
        INC(n, 2);
187
    ASSERT(x > ZERO)
-
 
188
    RETURN ln(x) / ln(base)
104
        x := x * x2;
189
END log;
-
 
190
 
Line 105... Line -...
105
        a := x / FLT(n);
-
 
106
        res := res + a
191
 
107
    UNTIL a < eps
-
 
108
 
-
 
109
    RETURN res * 2.0
192
PROCEDURE SinCos (x, y, sign: REAL): REAL;
Line -... Line 193...
-
 
193
CONST
-
 
194
    ymax =  210828714;
110
END ln;
195
    c1   =  3.1416015625E0;
111
 
196
    c2   = -8.908910206761537356617E-6;
-
 
197
    r1   = -0.16666666666666665052E+0;
112
 
198
    r2   =  0.83333333333331650314E-2;
113
PROCEDURE power* (base, exponent: REAL): REAL;
199
    r3   = -0.19841269841201840457E-3;
114
BEGIN
200
    r4   =  0.27557319210152756119E-5;
Line 115... Line 201...
115
    ASSERT(base > 0.0)
201
    r5   = -0.25052106798274584544E-7;
116
    RETURN exp(exponent * ln(base))
202
    r6   =  0.16058936490371589114E-9;
Line 117... Line 203...
117
END power;
203
    r7   = -0.76429178068910467734E-12;
118
 
204
    r8   =  0.27204790957888846175E-14;
-
 
205
 
119
 
206
VAR
-
 
207
    n: INTEGER;
120
PROCEDURE log* (base, x: REAL): REAL;
208
    xn, f, x1, g: REAL;
-
 
209
 
-
 
210
BEGIN
121
BEGIN
211
    ASSERT(y < FLT(ymax));
122
    ASSERT(base > 0.0);
212
 
Line -... Line 213...
-
 
213
    n := FLOOR(y * piInv + HALF);
-
 
214
    xn := FLT(n);
-
 
215
    IF ODD(n) THEN
-
 
216
        sign := -sign
-
 
217
    END;
123
    ASSERT(x > 0.0)
218
    x := ABS(x);
-
 
219
    IF x # y THEN
-
 
220
        xn := xn - HALF
-
 
221
    END;
124
    RETURN ln(x) / ln(base)
222
 
-
 
223
    x1 := FLT(FLOOR(x));
-
 
224
    f  := ((x1 - xn * c1) + (x - x1)) - xn * c2;
125
END log;
225
 
126
 
226
    IF ABS(f) < Limit THEN
127
 
227
        x := sign * f
-
 
228
    ELSE
128
PROCEDURE cos* (x: REAL): REAL;
229
        g := f * f;
Line -... Line 230...
-
 
230
        g := (((((((r8 * g + r7) * g + r6) * g + r5) * g + r4) * g + r3) * g + r2) * g + r1) * g;
-
 
231
        g := f + f * g;
Line 129... Line -...
129
VAR
-
 
Line -... Line 232...
-
 
232
        x := sign * g
-
 
233
    END
-
 
234
 
-
 
235
    RETURN x
-
 
236
END SinCos;
-
 
237
 
-
 
238
 
Line 130... Line -...
130
    a, res: REAL;
-
 
131
    n: INTEGER;
239
PROCEDURE sin* (x: REAL): REAL;
132
 
240
BEGIN
-
 
241
    IF x < ZERO THEN
Line 133... Line 242...
133
BEGIN
242
        x := SinCos(x, -x, -ONE)
-
 
243
    ELSE
134
    x := ABS(x);
244
        x := SinCos(x, x, ONE)
135
    ASSERT(x <= MaxCosArg);
245
    END
-
 
246
 
136
 
247
    RETURN x
-
 
248
END sin;
137
    x   := x - FLT( FLOOR(x / (2.0 * pi)) ) * (2.0 * pi);
249
 
-
 
250
 
138
    x   := x * x;
251
PROCEDURE cos* (x: REAL): REAL;
-
 
252
    RETURN SinCos(x, ABS(x) + piByTwo, ONE)
-
 
253
END cos;
-
 
254
 
-
 
255
 
139
    res := 0.0;
256
PROCEDURE tan* (x: REAL): REAL;
-
 
257
VAR
140
    a   := 1.0;
258
    s, c: REAL;
-
 
259
 
141
    n   := -1;
260
BEGIN
142
 
261
    s := sin(x);
-
 
262
    c := sqrt(ONE - s * s);
143
    REPEAT
263
    x := ABS(x) / (TWO * pi);
Line -... Line 264...
-
 
264
    x := x - FLT(FLOOR(x));
-
 
265
    IF (0.25 < x) & (x < 0.75) THEN
144
        INC(n, 2);
266
        c := -c
145
        res := res + a;
267
    END
Line -... Line 268...
-
 
268
 
-
 
269
    RETURN s / c
-
 
270
END tan;
-
 
271
 
-
 
272
 
-
 
273
PROCEDURE arctan2* (y, x: REAL): REAL;
-
 
274
CONST
-
 
275
    P0 = 0.216062307897242551884E+3;  P1 = 0.3226620700132512059245E+3;
Line -... Line 276...
-
 
276
    P2 = 0.13270239816397674701E+3;   P3 = 0.1288838303415727934E+2;
-
 
277
    Q0 = 0.2160623078972426128957E+3; Q1 = 0.3946828393122829592162E+3;
-
 
278
    Q2 = 0.221050883028417680623E+3;  Q3 = 0.3850148650835119501E+2;
-
 
279
    Sqrt3 = 1.7320508075688772935E0;
-
 
280
 
146
        a := -a * x / FLT(n*n + n)
281
VAR
147
    UNTIL ABS(a) < eps
282
    atan, z, z2, p, q: REAL;
Line 148... Line 283...
148
 
283
    yExp, xExp, Quadrant: INTEGER;
149
    RETURN res
-
 
150
END cos;
-
 
151
 
284
 
152
 
285
BEGIN
-
 
286
    IF ABS(x) < miny THEN
Line -... Line 287...
-
 
287
        ASSERT(ABS(y) >= miny);
-
 
288
        atan := piByTwo
-
 
289
    ELSE
-
 
290
        z := y;
153
PROCEDURE sin* (x: REAL): REAL;
291
        UNPK(z, yExp);
-
 
292
        z := x;
-
 
293
        UNPK(z, xExp);
-
 
294
 
-
 
295
        IF yExp - xExp >= expoMax - 3 THEN
-
 
296
            atan := piByTwo
-
 
297
        ELSIF yExp - xExp < expoMin + 3 THEN
-
 
298
            atan := ZERO
154
BEGIN
299
        ELSE
Line 155... Line 300...
155
    ASSERT(ABS(x) <= MaxCosArg);
300
            IF ABS(y) > ABS(x) THEN
156
    x := cos(x)
301
                z := ABS(x / y);
157
    RETURN sqrt(1.0 - x * x)
302
                Quadrant := 2
158
END sin;
303
            ELSE
159
 
304
                z := ABS(y / x);
Line 160... Line 305...
160
 
305
                Quadrant := 0
161
PROCEDURE tan* (x: REAL): REAL;
306
            END;
162
BEGIN
307
 
Line 163... Line 308...
163
    ASSERT(ABS(x) <= MaxCosArg);
308
            IF z > TWO - Sqrt3 THEN
164
    x := cos(x)
309
                z := (z * Sqrt3 - ONE) / (Sqrt3 + z);
165
    RETURN sqrt(1.0 - x * x) / x
310
                INC(Quadrant)
166
END tan;
311
            END;
167
 
312
 
Line 168... Line 313...
168
 
313
            IF ABS(z) < Limit THEN
169
PROCEDURE arcsin* (x: REAL): REAL;
314
                atan := z
170
 
315
            ELSE
171
 
316
                z2 := z * z;
172
    PROCEDURE arctan (x: REAL): REAL;
317
                p := (((P3 * z2 + P2) * z2 + P1) * z2 + P0) * z;
Line 173... Line 318...
173
    VAR
318
                q := (((z2 + Q3) * z2 + Q2) * z2 + Q1) * z2 + Q0;
174
        z, p, k: REAL;
319
                atan := p / q
175
 
320
            END;
176
    BEGIN
321
 
177
        p := x / (x * x + 1.0);
322
            CASE Quadrant OF
178
        z := p * x;
323
            |0:
179
        x := 0.0;
324
            |1: atan := atan + pi / 6.0
180
        k := 0.0;
325
            |2: atan := piByTwo - atan
181
 
326
            |3: atan := pi / 3.0 - atan
182
        REPEAT
327
            END
Line 183... Line 328...
183
            k := k + 2.0;
328
        END;
184
            x := x + p;
329
 
Line 185... Line 330...
185
            p := p * k * z / (k + 1.0)
330
        IF x < ZERO THEN
186
        UNTIL p < eps
331
            atan := pi - atan
187
 
332
        END
Line 188... Line 333...
188
        RETURN x
333
    END;
189
    END arctan;
334
 
190
 
335
    IF y < ZERO THEN
191
 
336
        atan := -atan
192
BEGIN
337
    END
Line 193... Line 338...
193
    ASSERT(ABS(x) <= 1.0);
338
 
194
 
339
    RETURN atan
195
    IF ABS(x) >= 0.707 THEN
340
END arctan2;
196
        x := 0.5 * pi - arctan(sqrt(1.0 - x * x) / x)
341
 
197
    ELSE
342
 
Line 198... Line 343...
198
        x := arctan(x / sqrt(1.0 - x * x))
343
PROCEDURE arcsin* (x: REAL): REAL;
199
    END
344
BEGIN
200
 
345
    ASSERT(ABS(x) <= ONE)
Line 201... Line 346...
201
    RETURN x
346
    RETURN arctan2(x, sqrt(ONE - x * x))
202
END arcsin;
347
END arcsin;
203
 
348
 
204
 
349
 
205
PROCEDURE arccos* (x: REAL): REAL;
350
PROCEDURE arccos* (x: REAL): REAL;
206
BEGIN
351
BEGIN
207
    ASSERT(ABS(x) <= 1.0)
352
    ASSERT(ABS(x) <= ONE)
208
    RETURN 0.5 * pi - arcsin(x)
353
    RETURN arctan2(sqrt(ONE - x * x), x)
Line 282... Line 427...
282
PROCEDURE fact* (n: INTEGER): REAL;
427
PROCEDURE fact* (n: INTEGER): REAL;
283
VAR
428
VAR
284
    res: REAL;
429
    res: REAL;
Line 285... Line 430...
285
 
430
 
286
BEGIN
431
BEGIN
287
    res := 1.0;
432
    res := ONE;
288
    WHILE n > 1 DO
433
    WHILE n > 1 DO
289
        res := res * FLT(n);
434
        res := res * FLT(n);
290
        DEC(n)
435
        DEC(n)
Line 291... Line 436...
291
    END
436
    END
292
 
437
 
Line -... Line 438...
-
 
438
    RETURN res
-
 
439
END fact;
-
 
440
 
-
 
441
 
-
 
442
PROCEDURE DegToRad* (x: REAL): REAL;
-
 
443
    RETURN x * (pi / 180.0)
-
 
444
END DegToRad;
293
    RETURN res
445
 
-
 
446
 
-
 
447
PROCEDURE RadToDeg* (x: REAL): REAL;
-
 
448
    RETURN x * (180.0 / pi)
-
 
449
END RadToDeg;
294
END fact;
450
 
295
 
451
 
Line 296... Line 452...
296
 
452
(* Return hypotenuse of triangle *)
297
PROCEDURE init;
453
PROCEDURE hypot* (x, y: REAL): REAL;
-
 
454
VAR
-
 
455
    a: REAL;
298
VAR
456
 
-
 
457
BEGIN
-
 
458
    x := ABS(x);
-
 
459
    y := ABS(y);
-
 
460
    IF x > y THEN
299
    i: INTEGER;
461
        a := x * sqrt(1.0 + sqrr(y / x))
-
 
462
    ELSE
300
 
463
        IF x > 0.0 THEN
-
 
464
            a := y * sqrt(1.0 + sqrr(x / y))
-
 
465
        ELSE
301
BEGIN
466
            a := y
Line 302... Line 467...
302
    Exp[0] := 1.0;
467
        END
303
    FOR i := 1 TO LEN(Exp) - 1 DO
468
    END
-
 
469
 
-
 
470
    RETURN a
-
 
471
END hypot;
-
 
472
 
304
        Exp[i] := Exp[i - 1] * e
473
 
305
    END
474
BEGIN