From 66288861241823271f75b3020e284bfa47a0e4b2 Mon Sep 17 00:00:00 2001
From: Wolfgang Ludwig <wolfgang.ludwig@esrf.fr>
Date: Tue, 5 Jun 2018 18:19:11 +0200
Subject: [PATCH] bugfix by Yohann Signed-off-by: Wolfgang Ludwig
<wolfgang.ludwig@esrf.fr>
---
zUtil_Maths/gtMathsRod2Euler.m | 101 +++++++++++++++++----------------
1 file changed, 52 insertions(+), 49 deletions(-)
mode change 100644 => 100755 zUtil_Maths/gtMathsRod2Euler.m
diff --git a/zUtil_Maths/gtMathsRod2Euler.m b/zUtil_Maths/gtMathsRod2Euler.m
old mode 100644
new mode 100755
index bfa7bf97..af615405
--- a/zUtil_Maths/gtMathsRod2Euler.m
+++ b/zUtil_Maths/gtMathsRod2Euler.m
@@ -1,49 +1,52 @@
-function euler = gtMathsRod2Euler(r)
-% GTMATHSROD2EULER Convert Rodrigues vectors to Euler angles (in degrees).
-% These follow Bunge convention (ZXZ) [phi1; phi; phi2].
-%
-% euler = gtMathsRod2Euler(r)
-% -------------------------------------------------------------------------
-%
-% INPUT:
-% r = <double> Rodrigues vectors, as column vectors (3xN array).
-%
-% OUTPUT:
-% euler = <double> Euler angles triplets, as column vectors (3xN array).
-% Output angles are in degrees and only positive.
-%
-% Version 002 17-10-2012 by YGuilhem
-% Vectorize function to process quickly lots of Rogrigues vectors
-%
-% Version 001 15-06-2012 by YGuilhem
-
-% Check size of r column vectors
-if size(r, 1) ~= 3
- gtError('Rodrigues:wrong_input_size', ...
- 'Input array r should be sized 3xN!');
-end
-
-% Set epsilon to avoid singular values
-epsilon = 1.e-9;
-
-% Build regular filters
-regular = r >= epsilon;
-regular_a = regular(1, :) & regular(2, :);
-regular_b = regular(3, :);
-
-% conpute intermediate values
-a = double(~regular_a).*pi/4 + double(regular_a).*atan2(r(2, :), r(1, :));
-b = double(~regular_b).*pi/4 + double(regular_b).*atan(r(3, :));
-
-% Compute Euler angles in radians
-phi1 = a + b;
-phi = 2*atan2(sqrt(r(1, :).^2 + r(2, :).^2), sqrt(1. + r(3, :).^2));
-phi2 = b - a;
-
-% Switch to degrees
-euler = [phi1 ; phi ; phi2] * 180 / pi;
-
-% Shift to positive angles range
-euler = mod(euler, 360.);
-
-end % end of function
+function euler = gtMathsRod2Euler(r)
+% GTMATHSROD2EULER Convert Rodrigues vectors to Euler angles (in degrees).
+% These follow Bunge convention (ZXZ) [phi1; phi; phi2].
+%
+% euler = gtMathsRod2Euler(r)
+% -------------------------------------------------------------------------
+%
+% INPUT:
+% r = <double> Rodrigues vectors, as column vectors (3xN array).
+%
+% OUTPUT:
+% euler = <double> Euler angles triplets, as column vectors (3xN array).
+% Output angles are in degrees and only positive.
+%
+% Version 002 17-10-2012 by YGuilhem
+% Vectorize function to process quickly lots of Rogrigues vectors
+%
+% Version 001 15-06-2012 by YGuilhem
+
+% Check size of r column vectors
+if size(r, 1) ~= 3
+ gtError('Rodrigues:wrong_input_size', ...
+ 'Input array r should be sized 3xN!');
+end
+
+% Set epsilon to avoid singular values
+epsilon = 1.e-9;
+
+% Compute first intermediate value
+c = cos(atan(sqrt(sum(r.^2))));
+
+% Build regular filters
+regular = abs(c.*r) >= epsilon;
+regular_a = regular(1, :) | regular(2, :);
+regular_b = regular(3, :) | c >= epsilon;
+
+% Compute second intermediate values
+a = double(~regular_a).*pi/4 + double(regular_a).*atan2(r(2, :), r(1, :));
+b = double(~regular_b).*pi/4 + double(regular_b).*atan2(r(3, :), 1.);
+
+% Compute Euler angles in radians
+phi1 = a + b;
+phi = 2*atan2(sqrt(r(1, :).^2 + r(2, :).^2), sqrt(1. + r(3, :).^2));
+phi2 = b - a;
+
+% Switch to degrees
+euler = [phi1 ; phi ; phi2] * 180 / pi;
+
+% Shift to positive angles range
+euler = mod(euler, 360.);
+
+end % end of function
--
GitLab