Newer
Older
Peter Reischig
committed
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
function angrad = gtMathsVectorsAnglesRad(v1, v2, colvec)
% GTMATHSVECTORSANGLESRAD Angles in RADIANS between 3D UNIT vectors.
%
% angrad = gtMathsVectorsAnglesRad(v1, v2, colvec)
%
% ----------------------------------------------------------------
%
% Angles in RADIANS between the input 3D UNIT VECTORS v1 and v2.
% The angle is computed with a asin function from the vectorial difference
% between two vectors (0 <= angrad <= pi/2):
% angrad = 2 * asin( 0.5*norm(v1-v2) )
%
% This is more accurate and less prone to numerical errors than using acos,
% while the computation time is similar.
%
% INPUT
% v1 - one or multiple arbitrary 3D UNIT VECTORS
% v2 - a set of arbitrary 3D UNIT VECTORS
% colvec - true if column vectors are used, false if row vectors:
% true - size(v1) is 3xn or 3x1; size(v2) is 3xn
% false - size(v1) is nx3 or 1x3; size(v2) is nx3
%
% OUTPUT
% angrad - angles between v1 and v2 in RADIANS; 0 <= angrad <= pi/2;
% (1xn or nx1)
%
if colvec
% COLUMN VECTORS
if size(v1,2)==1
% Sign of dot products to check whether the vectors are aligned or opposite
ss = sign(v1'*v2);
ss(ss==0) = 1;
% Extend v1 to the same size as v2
v1e = v1(:, ones(size(v2,2),1));
% Vectorial differences
dv = v2 - ss([1 1 1],:).*v1e;
else
% Sign of dot products to check whether the vectors are aligned or opposite
ss = sign(sum(v1.*v2, 1));
ss(ss==0) = 1;
% Vectorial differences
dv = v2 - ss([1 1 1],:).*v1;
end
% Angles in radian
angrad = 2*asin(0.5 * sqrt(sum(dv.^2, 1)));
else
% ROW VECTORS
if size(v1,1)==1
% Sign of dot products to check whether the vectors are aligned or opposite
ss = sign(v2*v1');
ss(ss==0) = 1;
% Extend v1 to the same size as v2
v1e = v1(ones(size(v2,1),1), :);
% Vectorial differences
dv = v2 - ss(:,[1 1 1]).*v1e;
else
% Sign of dot products to check whether the vectors are aligned or opposite
ss = sign(sum(v2.*v1, 2));
ss(ss==0) = 1;
% Vectorial differences
dv = v2 - ss(:,[1 1 1]).*v1;
end
% Angles in radian
angrad = 2*asin(0.5 * sqrt(sum(dv.^2, 2)));
end
end % of function