-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathmrpalt.m
62 lines (51 loc) · 1.6 KB
/
mrpalt.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
function p = mrpalt(p, f)
% MRPALT Shadow set of given MRPs
%
% Convert a set of modified Rodrigues parameters to its shadow set.
%
% p_shadow = MRPALT(p) % for traditional MRPs
% p_shadow = MRPALT(p, f) % for scaled MRPs
%
% Inputs:
%
% p Modified Rodrigues parameters (3-by-n)
% f Optional scaling parameter (default 1)
%
% Outputs:
%
% s Shadow set of the input modified Rodrigues parameters (3-by-n)
% Copyright 2016 An Uncommon Lab
%#codegen
% Set defaults so that for small angles, the scaled MRPs approach the
% rotation vector.
if nargin < 2 || isempty(f), f = 1; end;
% Check dimensions.
assert(nargin >= 1, ...
'%s: At least one input is required.', mfilename);
assert(size(p, 1) == 3, ...
'%s: The MRPs must be 3-by-n.', mfilename);
assert(all(size(f) == 1) && f > 0, ...
'%s: The scaling factor must be a positive scalar.', mfilename);
% MATLAB
if isempty(coder.target)
pm2 = p(1,:).*p(1,:) + p(2,:).*p(2,:) + p(3,:).*p(3,:);
nz = pm2 > 0;
c0 = (-f*f)./(pm2(nz));
p(1, nz) = c0 .* p(1, nz);
p(2, nz) = c0 .* p(2, nz);
p(3, nz) = c0 .* p(3, nz);
% codegen
else
n = size(p, 2);
nf2 = -f * f;
for k = 1:n
pm2 = p(1,k).*p(1,k) + p(2,k).*p(2,k) + p(3,k).*p(3,k);
if pm2 > 0
c0 = nf2 / pm2;
p(1,k) = c0 .* p(1,k);
p(2,k) = c0 .* p(2,k);
p(3,k) = c0 .* p(3,k);
end
end
end
end % mrpalt