Subversion Repositories Kolibri OS

Rev

Rev 7983 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
7983 leency 1
(*
2
    BSD 2-Clause License
3
 
8097 maxcodehac 4
    Copyright (c) 2019-2020, Anton Krotov
7983 leency 5
    All rights reserved.
6
*)
7
 
8
MODULE Math;
9
 
10
IMPORT SYSTEM;
11
 
12
 
13
CONST
14
 
8097 maxcodehac 15
    pi* = 3.1415926535897932384626433832795028841972E0;
16
    e*  = 2.7182818284590452353602874713526624977572E0;
7983 leency 17
 
8097 maxcodehac 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;
24
    ln2Inv    = 1.44269504088896340735992468100189213E0;
25
    piInv     = ONE / pi;
26
    Limit     = 1.0536712E-8;
27
    piByTwo   = pi / TWO;
7983 leency 28
 
8097 maxcodehac 29
    expoMax   = 1023;
30
    expoMin   = 1 - expoMax;
7983 leency 31
 
8097 maxcodehac 32
 
7983 leency 33
VAR
34
 
8097 maxcodehac 35
    LnInfinity, LnSmall, large, miny: REAL;
7983 leency 36
 
37
 
38
PROCEDURE [stdcall64] sqrt* (x: REAL): REAL;
39
BEGIN
8097 maxcodehac 40
    ASSERT(x >= ZERO);
7983 leency 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
    )
46
 
47
    RETURN 0.0
48
END sqrt;
49
 
50
 
8097 maxcodehac 51
PROCEDURE sqri* (x: INTEGER): INTEGER;
52
    RETURN x * x
53
END sqri;
54
 
55
 
56
PROCEDURE sqrr* (x: REAL): REAL;
57
    RETURN x * x
58
END sqrr;
59
 
60
 
7983 leency 61
PROCEDURE exp* (x: REAL): REAL;
62
CONST
8097 maxcodehac 63
    c1 =  0.693359375E0;
64
    c2 = -2.1219444005469058277E-4;
65
    P0 =  0.249999999999999993E+0;
66
    P1 =  0.694360001511792852E-2;
67
    P2 =  0.165203300268279130E-4;
68
    Q1 =  0.555538666969001188E-1;
69
    Q2 =  0.495862884905441294E-3;
7983 leency 70
 
71
VAR
8097 maxcodehac 72
    xn, g, p, q, z: REAL;
7983 leency 73
    n: INTEGER;
74
 
75
BEGIN
8097 maxcodehac 76
    IF x > LnInfinity THEN
77
        x := SYSTEM.INF()
78
    ELSIF x < LnSmall THEN
79
        x := ZERO
80
    ELSIF ABS(x) < eps THEN
81
        x := ONE
7983 leency 82
    ELSE
8097 maxcodehac 83
        IF x >= ZERO THEN
84
            n := FLOOR(ln2Inv * x + HALF)
85
        ELSE
86
            n := FLOOR(ln2Inv * x - HALF)
87
        END;
7983 leency 88
 
8097 maxcodehac 89
        xn := FLT(n);
90
        g  := (x - xn * c1) - xn * c2;
91
        z  := g * g;
92
        p  := ((P2 * z + P1) * z + P0) * g;
93
        q  := (Q2 * z + Q1) * z + HALF;
94
        x  := HALF + p / (q - p);
95
        PACK(x, n + 1)
7983 leency 96
    END
97
 
8097 maxcodehac 98
    RETURN x
7983 leency 99
END exp;
100
 
101
 
102
PROCEDURE ln* (x: REAL): REAL;
8097 maxcodehac 103
CONST
104
    c1 =  355.0E0 / 512.0E0;
105
    c2 = -2.121944400546905827679E-4;
106
    P0 = -0.64124943423745581147E+2;
107
    P1 =  0.16383943563021534222E+2;
108
    P2 = -0.78956112887491257267E+0;
109
    Q0 = -0.76949932108494879777E+3;
110
    Q1 =  0.31203222091924532844E+3;
111
    Q2 = -0.35667977739034646171E+2;
112
 
7983 leency 113
VAR
8097 maxcodehac 114
    zn, zd, r, z, w, p, q, xn: REAL;
7983 leency 115
    n: INTEGER;
116
 
117
BEGIN
8097 maxcodehac 118
    ASSERT(x > ZERO);
119
 
7983 leency 120
    UNPK(x, n);
8097 maxcodehac 121
    x := x * HALF;
7983 leency 122
 
8097 maxcodehac 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;
7983 leency 131
 
8097 maxcodehac 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)
7983 leency 138
 
8097 maxcodehac 139
    RETURN (xn * c2 + r) + xn * c1
7983 leency 140
END ln;
141
 
142
 
143
PROCEDURE power* (base, exponent: REAL): REAL;
144
BEGIN
8097 maxcodehac 145
    ASSERT(base > ZERO)
7983 leency 146
    RETURN exp(exponent * ln(base))
147
END power;
148
 
149
 
8097 maxcodehac 150
PROCEDURE ipower* (base: REAL; exponent: INTEGER): REAL;
151
VAR
152
    i: INTEGER;
153
    a: REAL;
154
 
155
BEGIN
156
    a := 1.0;
157
 
158
    IF base # 0.0 THEN
159
        IF exponent # 0 THEN
160
            IF exponent < 0 THEN
161
                base := 1.0 / base
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
176
        ASSERT(exponent > 0);
177
        a := 0.0
178
    END
179
 
180
    RETURN a
181
END ipower;
182
 
183
 
7983 leency 184
PROCEDURE log* (base, x: REAL): REAL;
185
BEGIN
8097 maxcodehac 186
    ASSERT(base > ZERO);
187
    ASSERT(x > ZERO)
7983 leency 188
    RETURN ln(x) / ln(base)
189
END log;
190
 
191
 
8097 maxcodehac 192
PROCEDURE SinCos (x, y, sign: REAL): REAL;
193
CONST
194
    ymax =  210828714;
195
    c1   =  3.1416015625E0;
196
    c2   = -8.908910206761537356617E-6;
197
    r1   = -0.16666666666666665052E+0;
198
    r2   =  0.83333333333331650314E-2;
199
    r3   = -0.19841269841201840457E-3;
200
    r4   =  0.27557319210152756119E-5;
201
    r5   = -0.25052106798274584544E-7;
202
    r6   =  0.16058936490371589114E-9;
203
    r7   = -0.76429178068910467734E-12;
204
    r8   =  0.27204790957888846175E-14;
205
 
7983 leency 206
VAR
207
    n: INTEGER;
8097 maxcodehac 208
    xn, f, x1, g: REAL;
7983 leency 209
 
210
BEGIN
8097 maxcodehac 211
    ASSERT(y < FLT(ymax));
212
 
213
    n := FLOOR(y * piInv + HALF);
214
    xn := FLT(n);
215
    IF ODD(n) THEN
216
        sign := -sign
217
    END;
7983 leency 218
    x := ABS(x);
8097 maxcodehac 219
    IF x # y THEN
220
        xn := xn - HALF
221
    END;
7983 leency 222
 
8097 maxcodehac 223
    x1 := FLT(FLOOR(x));
224
    f  := ((x1 - xn * c1) + (x - x1)) - xn * c2;
7983 leency 225
 
8097 maxcodehac 226
    IF ABS(f) < Limit THEN
227
        x := sign * f
228
    ELSE
229
        g := f * f;
230
        g := (((((((r8 * g + r7) * g + r6) * g + r5) * g + r4) * g + r3) * g + r2) * g + r1) * g;
231
        g := f + f * g;
232
        x := sign * g
233
    END
7983 leency 234
 
8097 maxcodehac 235
    RETURN x
236
END SinCos;
7983 leency 237
 
238
 
239
PROCEDURE sin* (x: REAL): REAL;
240
BEGIN
8097 maxcodehac 241
    IF x < ZERO THEN
242
        x := SinCos(x, -x, -ONE)
243
    ELSE
244
        x := SinCos(x, x, ONE)
245
    END
246
 
247
    RETURN x
7983 leency 248
END sin;
249
 
250
 
8097 maxcodehac 251
PROCEDURE cos* (x: REAL): REAL;
252
    RETURN SinCos(x, ABS(x) + piByTwo, ONE)
253
END cos;
254
 
255
 
7983 leency 256
PROCEDURE tan* (x: REAL): REAL;
8097 maxcodehac 257
VAR
258
    s, c: REAL;
259
 
7983 leency 260
BEGIN
8097 maxcodehac 261
    s := sin(x);
262
    c := sqrt(ONE - s * s);
263
    x := ABS(x) / (TWO * pi);
264
    x := x - FLT(FLOOR(x));
265
    IF (0.25 < x) & (x < 0.75) THEN
266
        c := -c
267
    END
268
 
269
    RETURN s / c
7983 leency 270
END tan;
271
 
272
 
8097 maxcodehac 273
PROCEDURE arctan2* (y, x: REAL): REAL;
274
CONST
275
    P0 = 0.216062307897242551884E+3;  P1 = 0.3226620700132512059245E+3;
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;
7983 leency 280
 
8097 maxcodehac 281
VAR
282
    atan, z, z2, p, q: REAL;
283
    yExp, xExp, Quadrant: INTEGER;
7983 leency 284
 
8097 maxcodehac 285
BEGIN
286
    IF ABS(x) < miny THEN
287
        ASSERT(ABS(y) >= miny);
288
        atan := piByTwo
289
    ELSE
290
        z := y;
291
        UNPK(z, yExp);
292
        z := x;
293
        UNPK(z, xExp);
7983 leency 294
 
8097 maxcodehac 295
        IF yExp - xExp >= expoMax - 3 THEN
296
            atan := piByTwo
297
        ELSIF yExp - xExp < expoMin + 3 THEN
298
            atan := ZERO
299
        ELSE
300
            IF ABS(y) > ABS(x) THEN
301
                z := ABS(x / y);
302
                Quadrant := 2
303
            ELSE
304
                z := ABS(y / x);
305
                Quadrant := 0
306
            END;
7983 leency 307
 
8097 maxcodehac 308
            IF z > TWO - Sqrt3 THEN
309
                z := (z * Sqrt3 - ONE) / (Sqrt3 + z);
310
                INC(Quadrant)
311
            END;
7983 leency 312
 
8097 maxcodehac 313
            IF ABS(z) < Limit THEN
314
                atan := z
315
            ELSE
316
                z2 := z * z;
317
                p := (((P3 * z2 + P2) * z2 + P1) * z2 + P0) * z;
318
                q := (((z2 + Q3) * z2 + Q2) * z2 + Q1) * z2 + Q0;
319
                atan := p / q
320
            END;
7983 leency 321
 
8097 maxcodehac 322
            CASE Quadrant OF
323
            |0:
324
            |1: atan := atan + pi / 6.0
325
            |2: atan := piByTwo - atan
326
            |3: atan := pi / 3.0 - atan
327
            END
328
        END;
7983 leency 329
 
8097 maxcodehac 330
        IF x < ZERO THEN
331
            atan := pi - atan
332
        END
333
    END;
7983 leency 334
 
8097 maxcodehac 335
    IF y < ZERO THEN
336
        atan := -atan
7983 leency 337
    END
338
 
8097 maxcodehac 339
    RETURN atan
340
END arctan2;
341
 
342
 
343
PROCEDURE arcsin* (x: REAL): REAL;
344
BEGIN
345
    ASSERT(ABS(x) <= ONE)
346
    RETURN arctan2(x, sqrt(ONE - x * x))
7983 leency 347
END arcsin;
348
 
349
 
350
PROCEDURE arccos* (x: REAL): REAL;
351
BEGIN
8097 maxcodehac 352
    ASSERT(ABS(x) <= ONE)
353
    RETURN arctan2(sqrt(ONE - x * x), x)
7983 leency 354
END arccos;
355
 
356
 
357
PROCEDURE arctan* (x: REAL): REAL;
8097 maxcodehac 358
    RETURN arctan2(x, ONE)
7983 leency 359
END arctan;
360
 
361
 
362
PROCEDURE sinh* (x: REAL): REAL;
363
BEGIN
364
    x := exp(x)
8097 maxcodehac 365
    RETURN (x - ONE / x) * HALF
7983 leency 366
END sinh;
367
 
368
 
369
PROCEDURE cosh* (x: REAL): REAL;
370
BEGIN
371
    x := exp(x)
8097 maxcodehac 372
    RETURN (x + ONE / x) * HALF
7983 leency 373
END cosh;
374
 
375
 
376
PROCEDURE tanh* (x: REAL): REAL;
377
BEGIN
378
    IF x > 15.0 THEN
8097 maxcodehac 379
        x := ONE
7983 leency 380
    ELSIF x < -15.0 THEN
8097 maxcodehac 381
        x := -ONE
7983 leency 382
    ELSE
8097 maxcodehac 383
        x := exp(TWO * x);
384
        x := (x - ONE) / (x + ONE)
7983 leency 385
    END
386
 
387
    RETURN x
388
END tanh;
389
 
390
 
391
PROCEDURE arsinh* (x: REAL): REAL;
8097 maxcodehac 392
    RETURN ln(x + sqrt(x * x + ONE))
7983 leency 393
END arsinh;
394
 
395
 
396
PROCEDURE arcosh* (x: REAL): REAL;
397
BEGIN
8097 maxcodehac 398
    ASSERT(x >= ONE)
399
    RETURN ln(x + sqrt(x * x - ONE))
7983 leency 400
END arcosh;
401
 
402
 
403
PROCEDURE artanh* (x: REAL): REAL;
404
BEGIN
8097 maxcodehac 405
    ASSERT(ABS(x) < ONE)
406
    RETURN HALF * ln((ONE + x) / (ONE - x))
7983 leency 407
END artanh;
408
 
409
 
410
PROCEDURE sgn* (x: REAL): INTEGER;
411
VAR
412
    res: INTEGER;
413
 
414
BEGIN
8097 maxcodehac 415
    IF x > ZERO THEN
7983 leency 416
        res := 1
8097 maxcodehac 417
    ELSIF x < ZERO THEN
7983 leency 418
        res := -1
419
    ELSE
420
        res := 0
421
    END
422
 
423
    RETURN res
424
END sgn;
425
 
426
 
427
PROCEDURE fact* (n: INTEGER): REAL;
428
VAR
429
    res: REAL;
430
 
431
BEGIN
8097 maxcodehac 432
    res := ONE;
7983 leency 433
    WHILE n > 1 DO
434
        res := res * FLT(n);
435
        DEC(n)
436
    END
437
 
438
    RETURN res
439
END fact;
440
 
441
 
8097 maxcodehac 442
PROCEDURE DegToRad* (x: REAL): REAL;
443
    RETURN x * (pi / 180.0)
444
END DegToRad;
445
 
446
 
447
PROCEDURE RadToDeg* (x: REAL): REAL;
448
    RETURN x * (180.0 / pi)
449
END RadToDeg;
450
 
451
 
452
(* Return hypotenuse of triangle *)
453
PROCEDURE hypot* (x, y: REAL): REAL;
7983 leency 454
VAR
8097 maxcodehac 455
    a: REAL;
7983 leency 456
 
457
BEGIN
8097 maxcodehac 458
    x := ABS(x);
459
    y := ABS(y);
460
    IF x > y THEN
461
        a := x * sqrt(1.0 + sqrr(y / x))
462
    ELSE
463
        IF x > 0.0 THEN
464
            a := y * sqrt(1.0 + sqrr(x / y))
465
        ELSE
466
            a := y
467
        END
7983 leency 468
    END
469
 
8097 maxcodehac 470
    RETURN a
471
END hypot;
7983 leency 472
 
8097 maxcodehac 473
 
7983 leency 474
BEGIN
8097 maxcodehac 475
    large := 1.9;
476
    PACK(large, expoMax);
477
    miny := ONE / large;
478
    LnInfinity := ln(large);
479
    LnSmall    := ln(miny);
7983 leency 480
END Math.