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
|
/* arcdot.c
*
* Angle between two vectors
*
*
*
*
* SYNOPSIS:
*
* long double p[3], q[3], arcdotl();
*
* y = arcdotl( p, q );
*
*
*
* DESCRIPTION:
*
* For two vectors p, q, the angle A between them is given by
*
* p.q / (|p| |q|) = cos A .
*
* where "." represents inner product, "|x|" the length of vector x.
* If the angle is small, an expression in sin A is preferred.
* Set r = q - p. Then
*
* p.q = p.p + p.r ,
*
* |p|^2 = p.p ,
*
* |q|^2 = p.p + 2 p.r + r.r ,
*
* p.p^2 + 2 p.p p.r + p.r^2
* cos^2 A = ----------------------------
* p.p (p.p + 2 p.r + r.r)
*
* p.p + 2 p.r + p.r^2 / p.p
* = --------------------------- ,
* p.p + 2 p.r + r.r
*
* sin^2 A = 1 - cos^2 A
*
* r.r - p.r^2 / p.p
* = --------------------
* p.p + 2 p.r + r.r
*
* = (r.r - p.r^2 / p.p) / q.q .
*
* ACCURACY:
*
* About 1 ULP. See arcdot.c.
*
*/
/*
Cephes Math Library Release 2.3: November, 1995
Copyright 1995 by Stephen L. Moshier
*/
#include <math.h>
#ifdef ANSIPROT
extern long double sqrtl ( long double );
extern long double acosl ( long double );
extern long double asinl ( long double );
extern long double atanl ( long double );
#else
long double sqrtl(), acosl(), asinl(), atanl();
#endif
extern long double PIL;
long double arcdotl(p,q)
long double p[], q[];
{
long double pp, pr, qq, rr, rt, pt, qt, pq;
int i;
pq = 0.0L;
qq = 0.0L;
pp = 0.0L;
pr = 0.0L;
rr = 0.0L;
for (i=0; i<3; i++)
{
pt = p[i];
qt = q[i];
pq += pt * qt;
qq += qt * qt;
pp += pt * pt;
rt = qt - pt;
pr += pt * rt;
rr += rt * rt;
}
if (rr == 0.0L || pp == 0.0L || qq == 0.0L)
return 0.0L;
rt = (rr - (pr * pr) / pp) / qq;
if (rt <= 0.75L)
{
rt = sqrtl(rt);
qt = asinl(rt);
if (pq < 0.0L)
qt = PIL - qt;
}
else
{
pt = pq / sqrtl(pp*qq);
qt = acosl(pt);
}
return qt;
}
|
