-
Notifications
You must be signed in to change notification settings - Fork 0
/
RotMatrix2Quat.asv
53 lines (46 loc) · 1.25 KB
/
RotMatrix2Quat.asv
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
thZ1 = 10*(pi/180);
thY = 10*(pi/180);
thZ3 = 10*(pi/180);
%Rotation about the Z axis
Rz1 = [ cos(thZ1) sin(thZ1) 0;...
-sin(thZ1) cos(thZ1) 0;...
0 0 1];
%Rotation about the Y axis
Ry = [ cos(thY) 0 -sin(thY);...
0 1 0;...
sin(thY) 0 cos(thY)];
%Rotation about the Z axis
Rz3 = [ cos(thZ3) sin(thZ3) 0;...
-sin(thZ3) cos(thZ3) 0;...
0 0 1];
R = Rz3*(Ry*Rz1);
q = euler2quat([thZ1 thY thZ3]);
t = trace(R);
if t > 0
q2 = [0;0;0;0];
S = 0.5 / sqrt(t);
q2(4) = 0.25 / S;
q2(1) = ( R(3,2) - R(2,3) ) * S;
q2(2) = ( R(1,3) - R(3,1) ) * S;
q2(3) = ( R(2,1) - R(1,2) ) * S;
else
if (m00 > m11)&&(m00 > m22)
S = sqrt( 1.0 + m00 - m11 - m22 ) * 2; // S=4*qx
qw = (m12 - m21) / S;
qx = 0.25 * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
elseif (m11 > m22)
S = sqrt( 1.0 + m11 - m00 - m22 ) * 2; // S=4*qy
qw = (m02 - m20) / S;
qx = (m01 + m10) / S;
qy = 0.25 * S;
qz = (m12 + m21) / S;
else
S = sqrt( 1.0 + m22 - m00 - m11 ) * 2; // S=4*qz
qw = (m01 - m10) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25 * S;
end
end