(*
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.