Eigenvalues and Eigenvectors of 3x3 symm matrices, added degeneracy handling

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

zUtil_Maths/gtMathsEig3x3SymmPosDef.m

-function [e, V] = gtMathsEig3x3SymmPosDef(M)
+function [e, V] = gtMathsEig3x3SymmPosDef(M, toll_deg)
+    if (~exist('toll_deg', 'var') || isempty(toll_deg))
+        toll_deg = 1e5;
+    end
     num_matrs = size(M, 3);
     e = zeros(1, 3, num_matrs, class(M));
 
@@ -47,6 +50,8 @@ function [e, V] = gtMathsEig3x3SymmPosDef(M)
         e(1, 2, others) = q_3 - e(1, 1, others) - e(1, 3, others);
     end
 
+    e = abs(e);
+
     V = zeros(3, 3, num_matrs, 'like', M);
 
     M23M12 = M(2, 3, :) .* M(1, 2, :);
@@ -59,6 +64,15 @@ function [e, V] = gtMathsEig3x3SymmPosDef(M)
     V(2, :, :) = bsxfun(@minus, bsxfun(@times, M(2, 3, :), M11), M12M13);
     V(3, :, :) = bsxfun(@minus, M12_2, M11 .* M22);
 
+    % Let's take care of the degeneracies
+    deg_12 = abs(e(1, 1, :) - e(1, 2, :)) < toll_deg;
+    deg_13 = abs(e(1, 1, :) - e(1, 3, :)) < toll_deg;
+    deg_23 = abs(e(1, 2, :) - e(1, 3, :)) < toll_deg;
+
+    V(:, 2, deg_12) = gtMathsCross(V(:, 1, deg_12), V(:, 3, deg_12), 1);
+    V(:, 3, deg_13) = gtMathsCross(V(:, 1, deg_13), V(:, 2, deg_13), 1);
+    V(:, 3, deg_23) = gtMathsCross(V(:, 1, deg_23), V(:, 2, deg_23), 1);
+
     norm_V = sqrt(sum(V .^ 2, 1));
     norm_V = 1 ./ (norm_V + (norm_V == 0));
     V = bsxfun(@times, V, norm_V);