Euler/Rodrigues spaces: added and improved a few functions

Signed-off-by: Nicola Vigano <nicola.vigano@esrf.fr>

zUtil_Maths/gtMathsEuler2OriMat.m

-function g = gtMathsEuler2OriMat(euler)
+function g = gtMathsEuler2OriMat(euler, convention)
 % GTMATHSEULER2ROTMAT  Convert orientation matrices to Euler angles (degrees).
 %                      These follow Bunge convention (ZXZ) [phi1; phi; phi2].
 %
@@ -23,26 +23,51 @@ if size(euler, 1) ~= 3
         'Input array euler should be sized 3xN!');
 end
 
+if (~exist('convention', 'var'))
+    convention = 'ZXZ';
+end
+
 % Get number of Euler triplets
 N = size(euler, 1);
 
 % Reshape angles components
 euler = reshape(euler, 3, 1, []);
-phi1 = euler(1, :, :);
-phi  = euler(2, :, :);
-phi2 = euler(3, :, :);
-
-% Compute intermediate values 
-c1 = cosd(phi1);
-cg = cosd(phi);
-c2 = cosd(phi2);
-s1 = sind(phi1);
-sg = sind(phi);
-s2 = sind(phi2);
-
-% Compute g matrices components
-g = [  c1.*c2 - s1.*cg.*s2 ,  s1.*c2 + c1.*cg.*s2 ,  sg.*s2 ; ...
-      -c1.*s2 - s1.*cg.*c2 ,  c1.*cg.*c2 - s1.*s2 ,  sg.*c2 ; ...
-       s1.*sg              , -c1.*sg              ,  cg     ];
+
+switch (convention)
+    case 'ZXZ'
+        phi1 = euler(1, :, :);
+        phi  = euler(2, :, :);
+        phi2 = euler(3, :, :);
+
+        % Compute intermediate values
+        c1 = cosd(phi1);
+        cg = cosd(phi);
+        c2 = cosd(phi2);
+        s1 = sind(phi1);
+        sg = sind(phi);
+        s2 = sind(phi2);
+
+        % Compute g matrices components
+        g = [  c1.*c2 - s1.*cg.*s2 ,  s1.*c2 + c1.*cg.*s2 ,  sg.*s2 ; ...
+              -c1.*s2 - s1.*cg.*c2 ,  c1.*cg.*c2 - s1.*s2 ,  sg.*c2 ; ...
+               s1.*sg              , -c1.*sg              ,  cg     ];
+    case 'XYZ'
+        phi = euler(1, :, :);
+        theta = euler(2, :, :);
+        psi = euler(3, :, :);
+
+        % Compute intermediate values
+        c_phi = cosd(phi);
+        c_th = cosd(theta);
+        c_psi = cosd(psi);
+        s_phi = sind(phi);
+        s_th = sind(theta);
+        s_psi = sind(psi);
+
+        % Compute g matrices components
+        g = [ c_th .* c_psi,                           c_th.*s_psi,                             -s_th; ...
+              s_phi .* s_th .* c_psi - c_phi .* s_psi, s_phi .* s_th .* s_psi + c_phi .* c_psi,  c_th .* s_phi; ...
+              c_phi .* s_th .* c_psi + s_phi .* s_psi, c_phi .* s_th .* s_psi - s_phi .* c_psi,  c_th .* c_phi ];
+end
 
 end % end of function

zUtil_Maths/gtMathsEuler2RodInFundZone.m

+function r_vec = gtMathsEuler2RodInFundZone(e_vec, symm)
+    g = gtMathsEuler2OriMat(e_vec);
+
+    num_symm_ops = numel(symm);
+
+    r_vecs = zeros(2 * num_symm_ops, 3);
+
+    for ii = 1:num_symm_ops
+        r_vecs((ii - 1) * 2 + 1, :) = gtMathsOriMat2Rod(symm(ii).g3 * g);
+        r_vecs((ii - 1) * 2 + 2, :) = gtMathsOriMat2Rod(symm(ii).g3' * g);
+    end
+
+    dists = 2 * atand(sqrt(sum(r_vecs .^ 2, 2)));
+
+    [~, ii] = min(dists);
+
+%     r_vecs
+
+    r_vec = r_vecs(ii, :);
+end

zUtil_Maths/gtMathsOriMat2Euler.m

@@ -43,7 +43,7 @@ phi2 =        double(singular).*phi1;
 phi2 = phi2 + double(regular ).*atan2(g(1, 3, :), g(2, 3, :));
 
 % Gather all Euler angles in an array
-euler = rad2deg([ phi1 ; phi ; phi2 ]);
+euler = [ phi1 ; phi ; phi2 ] * 180 / pi;
 
 % Shift to positive angles range
 euler = mod(euler, 360.);

zUtil_Maths/gtMathsRod2Euler.m

@@ -41,7 +41,7 @@ phi  = 2*atan2(sqrt(r(1, :).^2 + r(2, :).^2), sqrt(1. + r(3, :).^2));
 phi2 = b - a;
 
 % Switch to degrees
-euler = rad2deg([phi1 ; phi ; phi2]);
+euler = [phi1 ; phi ; phi2] * 180 / pi;
 
 % Shift to positive angles range
 euler = mod(euler, 360.);

zUtil_Maths/gtMathsRod2RodInFundZone.m

+function r_vec = gtMathsRod2RodInFundZone(r_vec, symm)
+    g = gtMathsRod2OriMat(r_vec);
+
+    num_symm_ops = numel(symm);
+
+    r_vecs = zeros(2 * num_symm_ops, 3);
+
+    for ii = 1:num_symm_ops
+        r_vecs((ii - 1) * 2 + 1, :) = gtMathsOriMat2Rod(symm(ii).g3 * g);
+        r_vecs((ii - 1) * 2 + 2, :) = gtMathsOriMat2Rod(symm(ii).g3' * g);
+    end
+
+    dists = 2 * atand(sqrt(sum(r_vecs .^ 2, 2)));
+
+    [~, ii] = min(dists);
+
+%     r_vecs
+
+    r_vec = r_vecs(ii, :);
+end