From 0fed9bf29c2a3d15913f625e4ca45a50fb3bea26 Mon Sep 17 00:00:00 2001
From: Nicola Vigano <nicola.vigano@esrf.fr>
Date: Wed, 6 May 2015 17:17:13 +0200
Subject: [PATCH] Euler/Rodrigues spaces: added and improved a few functions
Signed-off-by: Nicola Vigano <nicola.vigano@esrf.fr>
---
zUtil_Maths/gtMathsEuler2OriMat.m | 59 +++++++++++++++++-------
zUtil_Maths/gtMathsEuler2RodInFundZone.m | 20 ++++++++
zUtil_Maths/gtMathsOriMat2Euler.m | 2 +-
zUtil_Maths/gtMathsRod2Euler.m | 2 +-
zUtil_Maths/gtMathsRod2RodInFundZone.m | 20 ++++++++
5 files changed, 84 insertions(+), 19 deletions(-)
create mode 100755 zUtil_Maths/gtMathsEuler2RodInFundZone.m
create mode 100755 zUtil_Maths/gtMathsRod2RodInFundZone.m
diff --git a/zUtil_Maths/gtMathsEuler2OriMat.m b/zUtil_Maths/gtMathsEuler2OriMat.m
index 9ae36a20..7fa0eeff 100644
--- a/zUtil_Maths/gtMathsEuler2OriMat.m
+++ b/zUtil_Maths/gtMathsEuler2OriMat.m
@@ -1,4 +1,4 @@
-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
diff --git a/zUtil_Maths/gtMathsEuler2RodInFundZone.m b/zUtil_Maths/gtMathsEuler2RodInFundZone.m
new file mode 100755
index 00000000..95f0c576
--- /dev/null
+++ b/zUtil_Maths/gtMathsEuler2RodInFundZone.m
@@ -0,0 +1,20 @@
+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
diff --git a/zUtil_Maths/gtMathsOriMat2Euler.m b/zUtil_Maths/gtMathsOriMat2Euler.m
index afe02e39..7d89d979 100644
--- a/zUtil_Maths/gtMathsOriMat2Euler.m
+++ b/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.);
diff --git a/zUtil_Maths/gtMathsRod2Euler.m b/zUtil_Maths/gtMathsRod2Euler.m
index a4535841..bfa7bf97 100644
--- a/zUtil_Maths/gtMathsRod2Euler.m
+++ b/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.);
diff --git a/zUtil_Maths/gtMathsRod2RodInFundZone.m b/zUtil_Maths/gtMathsRod2RodInFundZone.m
new file mode 100755
index 00000000..f8e46305
--- /dev/null
+++ b/zUtil_Maths/gtMathsRod2RodInFundZone.m
@@ -0,0 +1,20 @@
+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
--
GitLab