-
Notifications
You must be signed in to change notification settings - Fork 2
/
q2mrp.m
71 lines (57 loc) · 1.83 KB
/
q2mrp.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
function p = q2mrp(q, f)
% Q2MRP Rotation quaternion to modified Rodrigues parameters
%
% Converts the rotation quaternion into modified Rodrigues parameters. For
% stability, the "near" MRPs, corresponding to a positive scalar part in
% the quaternion, will always be used.
%
% p = Q2MRP(q)
% p = Q2MRP(q, f)
%
% Inputs:
%
% q Rotation quaternion(s) (4-by-n)
% f MRP scaling factor (default 1)
%
% Outputs:
%
% p 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.
if size(q, 1) ~= 4 && size(q, 2) == 4, q = q.'; end;
assert(size(q, 1) == 4, ...
'%s: The quaternions must be 4-by-n.', mfilename);
assert(all(size(f) == 1) && f > 0, ...
'%s: The scaling factor must be a positive scalar.', mfilename);
% p = f * q(1:3) / (1 + q(4)); % individual quaternion, q(4) > 0
% p = -f * q(1:3) / (1 - q(4)); % if q(4) < 0
% Preallocate.
n = size(q, 2);
p = zeros(3, n, class(q));
% In MATLAB? Vectorize.
if isempty(coder.target)
% Create the coefficient so as to return only the "short way
% around" sets.
pos = q(4,:) > 0;
c0 = zeros(1, n, class(q));
c0( pos) = f ./ (1 + q(4, pos));
c0(~pos) = -f ./ (1 - q(4,~pos));
% Create the vector.
p(1,:) = c0 .* q(1,:);
p(2,:) = c0 .* q(2,:);
p(3,:) = c0 .* q(3,:);
% In code? Loop.
else
for k = 1:n
if q(4,k) > 0
p(:,k) = f / (1 + q(4,k)) * q(1:3,k);
else
p(:,k) = -f / (1 - q(4,k)) * q(1:3,k);
end
end
end
end % q2mrp