-
Notifications
You must be signed in to change notification settings - Fork 2
/
dcm2aa.m
129 lines (102 loc) · 4.45 KB
/
dcm2aa.m
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
function [theta, r] = dcm2aa(R)
% DCM2AA Direction cosine matrix to angle-axis representation
%
% Convert a direction cosine matrix (or several) to the corresponding angle
% of rotation and corresponding axes of rotation.
%
% [theta, r] = DCM2AA(R)
% theta = DCM2AA(R)
%
% When the axes are not needed, this output can be skipped, saving some
% computation time.
%
% Inputs:
%
% R Direction cosine matrices (3-by-3-by-n)
%
% Outputs:
%
% theta Angle(s) of rotation (radians, 1-by-n)
% r Unit axes of right-handed rotation (3-by-n)
% Copyright 2016 An Uncommon Lab
%#codegen
% Dims
assert(size(R, 1) == 3 && size(R, 2) == 3, ...
'%s: The axes must be 3-by-n.', mfilename);
% Angle of rotation
n = size(R, 3);
theta = zeros(1, n, class(R));
theta(:) = acos(0.5*(R(1,1,:) + R(2,2,:) + R(3,3,:) - 1));
% Axes of rotation
if nargout >= 2
% Preallocate.
r = zeros(3, n, class(R));
% If in MATLAB...
if isempty(coder.target)
theta_is_zero = theta == 0;
r(1, theta_is_zero) = 1;
r(2:end, theta_is_zero) = 0;
theta_is_pi = theta == pi;
use_column_1 = theta_is_pi ...
& reshape(R(1,1,:) >= R(3,3,:), [1 n]) ...
& reshape(R(1,1,:) >= R(2,2,:), [1 n]);
r(1,use_column_1) = R(1,1,use_column_1) + 1;
r(2,use_column_1) = R(2,1,use_column_1);
r(3,use_column_1) = R(3,1,use_column_1);
use_column_2 = theta_is_pi ...
& reshape(R(2,2,:) >= R(1,1,:), [1 n]) ...
& reshape(R(2,2,:) >= R(3,3,:), [1 n]);
r(1,use_column_2) = R(1,2,use_column_2);
r(2,use_column_2) = R(2,2,use_column_2) + 1;
r(3,use_column_2) = R(3,2,use_column_2);
use_column_3 = theta_is_pi & ~(use_column_1 | use_column_2);
r(1,use_column_3) = R(1,3,use_column_3);
r(2,use_column_3) = R(2,3,use_column_3);
r(3,use_column_3) = R(3,3,use_column_3) + 1;
r(:,theta_is_pi) = normalize(r(:,theta_is_pi));
normal = ~theta_is_zero & ~theta_is_pi;
if any(normal)
d = 1./(2 * sin(theta(normal)));
s = sum(normal);
r(1,normal) = d .* reshape(R(2,3,normal) - R(3,2,normal), [1 s]);
r(2,normal) = d .* reshape(R(3,1,normal) - R(1,3,normal), [1 s]);
r(3,normal) = d .* reshape(R(1,2,normal) - R(2,1,normal), [1 s]);
end
% Otherwise, codegen...
else
for k = 1:n
% If there's no rotation, then the axis is arbitrary.
if theta(k) == 0
r(1,k) = 1; % (The rest are already zeros.)
% If the rotation is half a circle, then the rotation axis
% will be proportional to a column of eye(3) + R. Choose
% the largest column and normalize for the axis.
elseif theta(k) == pi
if R(1,1,k) >= R(3,3,k)
if R(1,1,k) >= R(2,2,k)
r(1,k) = R(1,1,k) + 1;
r(2,k) = R(2,1,k);
r(3,k) = R(3,1,k);
else
r(1,k) = R(1,2,k);
r(2,k) = R(2,2,k) + 1;
r(3,k) = R(3,2,k);
end
else
r(1,k) = R(1,3,k);
r(2,k) = R(2,3,k);
r(3,k) = R(3,3,k) + 1;
end
r(:,k) = normalize(r(:,k));
% Otherwise, we build the rotation axis from the
% off-diagonal components of R.
else
d = 1/(2 * sin(theta(k)));
r(1,k) = d * (R(2,3,k) - R(3,2,k));
r(2,k) = d * (R(3,1,k) - R(1,3,k));
r(3,k) = d * (R(1,2,k) - R(2,1,k));
end
end
end
end
end % dcm2aa