1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
{ ******************************************************************
Trigonometric functions
****************************************************************** }
unit utrigo;
interface
uses
utypes, uminmax;
function Pythag(X, Y : Float) : Float; { Sqrt(X^2 + Y^2) }
function FixAngle(Theta : Float) : Float; { Set Theta in -Pi..Pi }
function Tan(X : Float) : Float; { Tangent }
function ArcSin(X : Float) : Float; { Arc sinus }
function ArcCos(X : Float) : Float; { Arc cosinus }
function ArcTan2(Y, X : Float) : Float; { Angle (Ox, OM) with M(X,Y) }
implementation
function Pythag(X, Y : Float) : Float;
{ Computes Sqrt(X^2 + Y^2) without destructive underflow or overflow }
var
AbsX, AbsY : Float;
begin
SetErrCode(FOk);
AbsX := Abs(X);
AbsY := Abs(Y);
if AbsX > AbsY then
Pythag := AbsX * Sqrt(1.0 + Sqr(AbsY / AbsX))
else if AbsY = 0.0 then
Pythag := 0.0
else
Pythag := AbsY * Sqrt(1.0 + Sqr(AbsX / AbsY));
end;
function FixAngle(Theta : Float) : Float;
begin
SetErrCode(FOk);
while Theta > Pi do
Theta := Theta - TwoPi;
while Theta <= - PI do
Theta := Theta + TwoPi;
FixAngle := Theta;
end;
function Tan(X : Float) : Float;
var
SinX, CosX : Float;
begin
SetErrCode(FOk);
SinX := Sin(X);
CosX := Cos(X);
if CosX = 0.0 then
Tan := DefaultVal(FSing, Sgn(SinX) * MaxNum)
else
Tan := SinX / CosX;
end;
function ArcSin(X : Float) : Float;
begin
SetErrCode(FOk);
if (X < - 1.0) or (X > 1.0) then
ArcSin := DefaultVal(FDomain, 0.0)
else if X = 1.0 then
ArcSin := PiDiv2
else if X = - 1.0 then
ArcSin := - PiDiv2
else
ArcSin := ArcTan(X / Sqrt(1.0 - Sqr(X)));
end;
function ArcCos(X : Float) : Float;
begin
SetErrCode(FOk);
if (X < - 1.0) or (X > 1.0) then
ArcCos := DefaultVal(FDomain, 0.0)
else if X = 1.0 then
ArcCos := 0.0
else if X = - 1.0 then
ArcCos := Pi
else
ArcCos := PiDiv2 - ArcTan(X / Sqrt(1.0 - Sqr(X)));
end;
function ArcTan2(Y, X : Float) : Float;
var
Theta : Float;
begin
SetErrCode(FOk);
if X = 0.0 then
if Y = 0.0 then
ArcTan2 := 0.0
else if Y > 0.0 then
ArcTan2 := PiDiv2
else
ArcTan2 := - PiDiv2
else
begin
{ 4th/1st quadrant -Pi/2..Pi/2 }
Theta := ArcTan(Y / X);
{ 2nd/3rd quadrants }
if X < 0.0 then
if Y >= 0.0 then
Theta := Theta + Pi { 2nd quadrant: Pi/2..Pi }
else
Theta := Theta - Pi; { 3rd quadrant: -Pi..-Pi/2 }
ArcTan2 := Theta;
end;
end;
end.
|