480 lines
8.9 KiB
Plaintext
480 lines
8.9 KiB
Plaintext
(*
|
|
BSD 2-Clause License
|
|
|
|
Copyright (c) 2019-2020, Anton Krotov
|
|
All rights reserved.
|
|
*)
|
|
|
|
MODULE Math;
|
|
|
|
IMPORT SYSTEM;
|
|
|
|
|
|
CONST
|
|
|
|
pi* = 3.1415926535897932384626433832795028841972E0;
|
|
e* = 2.7182818284590452353602874713526624977572E0;
|
|
|
|
ZERO = 0.0E0;
|
|
ONE = 1.0E0;
|
|
HALF = 0.5E0;
|
|
TWO = 2.0E0;
|
|
sqrtHalf = 0.70710678118654752440E0;
|
|
eps = 5.5511151E-17;
|
|
ln2Inv = 1.44269504088896340735992468100189213E0;
|
|
piInv = ONE / pi;
|
|
Limit = 1.0536712E-8;
|
|
piByTwo = pi / TWO;
|
|
|
|
expoMax = 1023;
|
|
expoMin = 1 - expoMax;
|
|
|
|
|
|
VAR
|
|
|
|
LnInfinity, LnSmall, large, miny: REAL;
|
|
|
|
|
|
PROCEDURE [stdcall64] sqrt* (x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(x >= ZERO);
|
|
SYSTEM.CODE(
|
|
0F2H, 0FH, 51H, 45H, 10H, (* sqrtsd xmm0, qword[rbp + 10h] *)
|
|
05DH, (* pop rbp *)
|
|
0C2H, 08H, 00H (* ret 8 *)
|
|
)
|
|
|
|
RETURN 0.0
|
|
END sqrt;
|
|
|
|
|
|
PROCEDURE sqri* (x: INTEGER): INTEGER;
|
|
RETURN x * x
|
|
END sqri;
|
|
|
|
|
|
PROCEDURE sqrr* (x: REAL): REAL;
|
|
RETURN x * x
|
|
END sqrr;
|
|
|
|
|
|
PROCEDURE exp* (x: REAL): REAL;
|
|
CONST
|
|
c1 = 0.693359375E0;
|
|
c2 = -2.1219444005469058277E-4;
|
|
P0 = 0.249999999999999993E+0;
|
|
P1 = 0.694360001511792852E-2;
|
|
P2 = 0.165203300268279130E-4;
|
|
Q1 = 0.555538666969001188E-1;
|
|
Q2 = 0.495862884905441294E-3;
|
|
|
|
VAR
|
|
xn, g, p, q, z: REAL;
|
|
n: INTEGER;
|
|
|
|
BEGIN
|
|
IF x > LnInfinity THEN
|
|
x := SYSTEM.INF()
|
|
ELSIF x < LnSmall THEN
|
|
x := ZERO
|
|
ELSIF ABS(x) < eps THEN
|
|
x := ONE
|
|
ELSE
|
|
IF x >= ZERO THEN
|
|
n := FLOOR(ln2Inv * x + HALF)
|
|
ELSE
|
|
n := FLOOR(ln2Inv * x - HALF)
|
|
END;
|
|
|
|
xn := FLT(n);
|
|
g := (x - xn * c1) - xn * c2;
|
|
z := g * g;
|
|
p := ((P2 * z + P1) * z + P0) * g;
|
|
q := (Q2 * z + Q1) * z + HALF;
|
|
x := HALF + p / (q - p);
|
|
PACK(x, n + 1)
|
|
END
|
|
|
|
RETURN x
|
|
END exp;
|
|
|
|
|
|
PROCEDURE ln* (x: REAL): REAL;
|
|
CONST
|
|
c1 = 355.0E0 / 512.0E0;
|
|
c2 = -2.121944400546905827679E-4;
|
|
P0 = -0.64124943423745581147E+2;
|
|
P1 = 0.16383943563021534222E+2;
|
|
P2 = -0.78956112887491257267E+0;
|
|
Q0 = -0.76949932108494879777E+3;
|
|
Q1 = 0.31203222091924532844E+3;
|
|
Q2 = -0.35667977739034646171E+2;
|
|
|
|
VAR
|
|
zn, zd, r, z, w, p, q, xn: REAL;
|
|
n: INTEGER;
|
|
|
|
BEGIN
|
|
ASSERT(x > ZERO);
|
|
|
|
UNPK(x, n);
|
|
x := x * HALF;
|
|
|
|
IF x > sqrtHalf THEN
|
|
zn := x - ONE;
|
|
zd := x * HALF + HALF;
|
|
INC(n)
|
|
ELSE
|
|
zn := x - HALF;
|
|
zd := zn * HALF + HALF
|
|
END;
|
|
|
|
z := zn / zd;
|
|
w := z * z;
|
|
q := ((w + Q2) * w + Q1) * w + Q0;
|
|
p := w * ((P2 * w + P1) * w + P0);
|
|
r := z + z * (p / q);
|
|
xn := FLT(n)
|
|
|
|
RETURN (xn * c2 + r) + xn * c1
|
|
END ln;
|
|
|
|
|
|
PROCEDURE power* (base, exponent: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(base > ZERO)
|
|
RETURN exp(exponent * ln(base))
|
|
END power;
|
|
|
|
|
|
PROCEDURE ipower* (base: REAL; exponent: INTEGER): REAL;
|
|
VAR
|
|
i: INTEGER;
|
|
a: REAL;
|
|
|
|
BEGIN
|
|
a := 1.0;
|
|
|
|
IF base # 0.0 THEN
|
|
IF exponent # 0 THEN
|
|
IF exponent < 0 THEN
|
|
base := 1.0 / base
|
|
END;
|
|
i := ABS(exponent);
|
|
WHILE i > 0 DO
|
|
WHILE ~ODD(i) DO
|
|
i := LSR(i, 1);
|
|
base := sqrr(base)
|
|
END;
|
|
DEC(i);
|
|
a := a * base
|
|
END
|
|
ELSE
|
|
a := 1.0
|
|
END
|
|
ELSE
|
|
ASSERT(exponent > 0);
|
|
a := 0.0
|
|
END
|
|
|
|
RETURN a
|
|
END ipower;
|
|
|
|
|
|
PROCEDURE log* (base, x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(base > ZERO);
|
|
ASSERT(x > ZERO)
|
|
RETURN ln(x) / ln(base)
|
|
END log;
|
|
|
|
|
|
PROCEDURE SinCos (x, y, sign: REAL): REAL;
|
|
CONST
|
|
ymax = 210828714;
|
|
c1 = 3.1416015625E0;
|
|
c2 = -8.908910206761537356617E-6;
|
|
r1 = -0.16666666666666665052E+0;
|
|
r2 = 0.83333333333331650314E-2;
|
|
r3 = -0.19841269841201840457E-3;
|
|
r4 = 0.27557319210152756119E-5;
|
|
r5 = -0.25052106798274584544E-7;
|
|
r6 = 0.16058936490371589114E-9;
|
|
r7 = -0.76429178068910467734E-12;
|
|
r8 = 0.27204790957888846175E-14;
|
|
|
|
VAR
|
|
n: INTEGER;
|
|
xn, f, x1, g: REAL;
|
|
|
|
BEGIN
|
|
ASSERT(y < FLT(ymax));
|
|
|
|
n := FLOOR(y * piInv + HALF);
|
|
xn := FLT(n);
|
|
IF ODD(n) THEN
|
|
sign := -sign
|
|
END;
|
|
x := ABS(x);
|
|
IF x # y THEN
|
|
xn := xn - HALF
|
|
END;
|
|
|
|
x1 := FLT(FLOOR(x));
|
|
f := ((x1 - xn * c1) + (x - x1)) - xn * c2;
|
|
|
|
IF ABS(f) < Limit THEN
|
|
x := sign * f
|
|
ELSE
|
|
g := f * f;
|
|
g := (((((((r8 * g + r7) * g + r6) * g + r5) * g + r4) * g + r3) * g + r2) * g + r1) * g;
|
|
g := f + f * g;
|
|
x := sign * g
|
|
END
|
|
|
|
RETURN x
|
|
END SinCos;
|
|
|
|
|
|
PROCEDURE sin* (x: REAL): REAL;
|
|
BEGIN
|
|
IF x < ZERO THEN
|
|
x := SinCos(x, -x, -ONE)
|
|
ELSE
|
|
x := SinCos(x, x, ONE)
|
|
END
|
|
|
|
RETURN x
|
|
END sin;
|
|
|
|
|
|
PROCEDURE cos* (x: REAL): REAL;
|
|
RETURN SinCos(x, ABS(x) + piByTwo, ONE)
|
|
END cos;
|
|
|
|
|
|
PROCEDURE tan* (x: REAL): REAL;
|
|
VAR
|
|
s, c: REAL;
|
|
|
|
BEGIN
|
|
s := sin(x);
|
|
c := sqrt(ONE - s * s);
|
|
x := ABS(x) / (TWO * pi);
|
|
x := x - FLT(FLOOR(x));
|
|
IF (0.25 < x) & (x < 0.75) THEN
|
|
c := -c
|
|
END
|
|
|
|
RETURN s / c
|
|
END tan;
|
|
|
|
|
|
PROCEDURE arctan2* (y, x: REAL): REAL;
|
|
CONST
|
|
P0 = 0.216062307897242551884E+3; P1 = 0.3226620700132512059245E+3;
|
|
P2 = 0.13270239816397674701E+3; P3 = 0.1288838303415727934E+2;
|
|
Q0 = 0.2160623078972426128957E+3; Q1 = 0.3946828393122829592162E+3;
|
|
Q2 = 0.221050883028417680623E+3; Q3 = 0.3850148650835119501E+2;
|
|
Sqrt3 = 1.7320508075688772935E0;
|
|
|
|
VAR
|
|
atan, z, z2, p, q: REAL;
|
|
yExp, xExp, Quadrant: INTEGER;
|
|
|
|
BEGIN
|
|
IF ABS(x) < miny THEN
|
|
ASSERT(ABS(y) >= miny);
|
|
atan := piByTwo
|
|
ELSE
|
|
z := y;
|
|
UNPK(z, yExp);
|
|
z := x;
|
|
UNPK(z, xExp);
|
|
|
|
IF yExp - xExp >= expoMax - 3 THEN
|
|
atan := piByTwo
|
|
ELSIF yExp - xExp < expoMin + 3 THEN
|
|
atan := ZERO
|
|
ELSE
|
|
IF ABS(y) > ABS(x) THEN
|
|
z := ABS(x / y);
|
|
Quadrant := 2
|
|
ELSE
|
|
z := ABS(y / x);
|
|
Quadrant := 0
|
|
END;
|
|
|
|
IF z > TWO - Sqrt3 THEN
|
|
z := (z * Sqrt3 - ONE) / (Sqrt3 + z);
|
|
INC(Quadrant)
|
|
END;
|
|
|
|
IF ABS(z) < Limit THEN
|
|
atan := z
|
|
ELSE
|
|
z2 := z * z;
|
|
p := (((P3 * z2 + P2) * z2 + P1) * z2 + P0) * z;
|
|
q := (((z2 + Q3) * z2 + Q2) * z2 + Q1) * z2 + Q0;
|
|
atan := p / q
|
|
END;
|
|
|
|
CASE Quadrant OF
|
|
|0:
|
|
|1: atan := atan + pi / 6.0
|
|
|2: atan := piByTwo - atan
|
|
|3: atan := pi / 3.0 - atan
|
|
END
|
|
END;
|
|
|
|
IF x < ZERO THEN
|
|
atan := pi - atan
|
|
END
|
|
END;
|
|
|
|
IF y < ZERO THEN
|
|
atan := -atan
|
|
END
|
|
|
|
RETURN atan
|
|
END arctan2;
|
|
|
|
|
|
PROCEDURE arcsin* (x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(ABS(x) <= ONE)
|
|
RETURN arctan2(x, sqrt(ONE - x * x))
|
|
END arcsin;
|
|
|
|
|
|
PROCEDURE arccos* (x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(ABS(x) <= ONE)
|
|
RETURN arctan2(sqrt(ONE - x * x), x)
|
|
END arccos;
|
|
|
|
|
|
PROCEDURE arctan* (x: REAL): REAL;
|
|
RETURN arctan2(x, ONE)
|
|
END arctan;
|
|
|
|
|
|
PROCEDURE sinh* (x: REAL): REAL;
|
|
BEGIN
|
|
x := exp(x)
|
|
RETURN (x - ONE / x) * HALF
|
|
END sinh;
|
|
|
|
|
|
PROCEDURE cosh* (x: REAL): REAL;
|
|
BEGIN
|
|
x := exp(x)
|
|
RETURN (x + ONE / x) * HALF
|
|
END cosh;
|
|
|
|
|
|
PROCEDURE tanh* (x: REAL): REAL;
|
|
BEGIN
|
|
IF x > 15.0 THEN
|
|
x := ONE
|
|
ELSIF x < -15.0 THEN
|
|
x := -ONE
|
|
ELSE
|
|
x := exp(TWO * x);
|
|
x := (x - ONE) / (x + ONE)
|
|
END
|
|
|
|
RETURN x
|
|
END tanh;
|
|
|
|
|
|
PROCEDURE arsinh* (x: REAL): REAL;
|
|
RETURN ln(x + sqrt(x * x + ONE))
|
|
END arsinh;
|
|
|
|
|
|
PROCEDURE arcosh* (x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(x >= ONE)
|
|
RETURN ln(x + sqrt(x * x - ONE))
|
|
END arcosh;
|
|
|
|
|
|
PROCEDURE artanh* (x: REAL): REAL;
|
|
BEGIN
|
|
ASSERT(ABS(x) < ONE)
|
|
RETURN HALF * ln((ONE + x) / (ONE - x))
|
|
END artanh;
|
|
|
|
|
|
PROCEDURE sgn* (x: REAL): INTEGER;
|
|
VAR
|
|
res: INTEGER;
|
|
|
|
BEGIN
|
|
IF x > ZERO THEN
|
|
res := 1
|
|
ELSIF x < ZERO THEN
|
|
res := -1
|
|
ELSE
|
|
res := 0
|
|
END
|
|
|
|
RETURN res
|
|
END sgn;
|
|
|
|
|
|
PROCEDURE fact* (n: INTEGER): REAL;
|
|
VAR
|
|
res: REAL;
|
|
|
|
BEGIN
|
|
res := ONE;
|
|
WHILE n > 1 DO
|
|
res := res * FLT(n);
|
|
DEC(n)
|
|
END
|
|
|
|
RETURN res
|
|
END fact;
|
|
|
|
|
|
PROCEDURE DegToRad* (x: REAL): REAL;
|
|
RETURN x * (pi / 180.0)
|
|
END DegToRad;
|
|
|
|
|
|
PROCEDURE RadToDeg* (x: REAL): REAL;
|
|
RETURN x * (180.0 / pi)
|
|
END RadToDeg;
|
|
|
|
|
|
(* Return hypotenuse of triangle *)
|
|
PROCEDURE hypot* (x, y: REAL): REAL;
|
|
VAR
|
|
a: REAL;
|
|
|
|
BEGIN
|
|
x := ABS(x);
|
|
y := ABS(y);
|
|
IF x > y THEN
|
|
a := x * sqrt(1.0 + sqrr(y / x))
|
|
ELSE
|
|
IF x > 0.0 THEN
|
|
a := y * sqrt(1.0 + sqrr(x / y))
|
|
ELSE
|
|
a := y
|
|
END
|
|
END
|
|
|
|
RETURN a
|
|
END hypot;
|
|
|
|
|
|
BEGIN
|
|
large := 1.9;
|
|
PACK(large, expoMax);
|
|
miny := ONE / large;
|
|
LnInfinity := ln(large);
|
|
LnSmall := ln(miny);
|
|
END Math. |