1function q = fromRotMat(R)
2%ROTATE convert normalized quaternion q to rotation matrix
4if size(R)~=[3 3]
5 error('Only a single rotation matrix can be processed');
6end
8% fix 'almost zero' or 'almost 1' elements of the rotation matrix
9roundedR = round( R );
10boolRound = abs( roundedR - R ) <= eps; % defines which values should be fixed
11R(boolRound) = roundedR(boolRound);
13if all(R == 0)
14 error('Empty (or almost empty) matrix provided in place of a rotation matrix');
15end
17qs = 0.5 * sqrt( max( 0, R(1,1) + R(2,2) + R(3,3) + 1 ));
19if abs(qs) <= eps
20 sgn_R23 = 1; sgn_R23(-R(2,3) < 0) = -1;
21 sgn_R13 = 1; sgn_R13(-R(1,3) < 0) = -1;
22 sgn_R12 = 1; sgn_R12(-R(1,2) < 0) = -1;
24 q = [0 sqrt( max( 0, -0.5 *( R(2,2) + R(3,3) ))) * sgn_R23 ...
25 sqrt( max( 0, -0.5 *( R(1,1) + R(3,3) ))) * sgn_R13 ...
26 sqrt( max( 0, -0.5 *( R(1,1) + R(2,2) ))) * sgn_R12 ];
27else
28 q = [qs 0.25 *( R(3,2) - R(2,3) ) / qs ...
29 0.25 *( R(1,3) - R(3,1) ) / qs ...
30 0.25 *( R(2,1) - R(1,2) ) / qs ];
31end
33% normalize the quaternion to account for rounding errors in the rotation matrix, etc
34q = mr.aux.quat.normalize(q);