gtMathsRotationMatrixComp: extended to produce multiple rotmatrix components...

gtMathsRotationMatrixComp: extended to produce multiple rotmatrix components for multiple rotdir as input

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

zUtil_Maths/gtMathsRotationMatrixComp.m

@@ -26,34 +26,44 @@ function rotmatcomp = gtMathsRotationMatrixComp(rotdir, rc)
 %
 %     INPUT
 %       rotdir - rotation axis direction in LAB coordinates for right-handed
-%                rotation; unit vector (1x3 for row, 3x1 for column vectors)
+%                rotation; unit vector (1x3xn for row, 3x1xn for column vectors)
 %       rc     - 'row' for row vectors, 'col' for column vectors
 %
 %     OUTPUT
-%       rotmatcomp.const = matrix Sconst (3x3)
-%       rotmatcomp.cos   = matrix Scos   (3x3)
-%       rotmatcomp.sin   = matrix Ssin   (3x3)
-
-if strcmp(rc,'row') && (size(rotdir,1) == 1) && (size(rotdir,2) == 3)
-
-    rotmatcomp.const = rotdir' * rotdir;
-    rotmatcomp.cos   = eye(3) - rotmatcomp.const;
-    rotmatcomp.sin   = [         0,  rotdir(3), -rotdir(2) ; ...
-                        -rotdir(3),          0,  rotdir(1) ; ...
-                         rotdir(2), -rotdir(1),          0 ];
-
-                     
-elseif strcmp(rc,'col') && (size(rotdir,1) == 3) && (size(rotdir,2) == 1)
-   
-    rotmatcomp.const = rotdir * rotdir';
-    rotmatcomp.cos   = eye(3) - rotmatcomp.const;
-    rotmatcomp.sin   = [         0, -rotdir(3),  rotdir(2) ; ...
-                         rotdir(3),          0, -rotdir(1) ; ...
-                        -rotdir(2),  rotdir(1),          0 ];
-    
-else
-    error('Wrong input format. See function help.')
-end
+%       rotmatcomp.const = matrix Sconst (3x3xn)
+%       rotmatcomp.cos   = matrix Scos   (3x3xn)
+%       rotmatcomp.sin   = matrix Ssin   (3x3xn)
+
+    num_vecs = size(rotdir, 3);
+
+    I = eye(3);
+    I = I(:, :, ones(num_vecs, 1));
+
+    z = zeros(1, 1, num_vecs);
+
+    rotdir_perm = permute(rotdir, [2 1 3]);
 
+    if (strcmp(rc, 'row') && (size(rotdir, 1) == 1) ...
+            && (size(rotdir, 2) == 3))
 
-end % end of function
+        rotmatcomp.const = rotdir_perm(:, [1 1 1], :) .* rotdir([1 1 1], :, :);
+
+        rotmatcomp.cos = I - rotmatcomp.const;
+        rotmatcomp.sin = [ ...
+                           z,  rotdir(1, 3, :), -rotdir(1, 2, :) ; ...
+            -rotdir(1, 3, :),                z,  rotdir(1, 1, :) ; ...
+             rotdir(1, 2, :), -rotdir(1, 1, :),                z ];
+    elseif (strcmp(rc, 'col') && (size(rotdir, 1) == 3) ...
+            && (size(rotdir, 2) == 1))
+
+        rotmatcomp.const = rotdir_perm([1 1 1], :, :) .* rotdir(:, [1 1 1], :);
+
+        rotmatcomp.cos = I - rotmatcomp.const;
+        rotmatcomp.sin = [ ...
+                           z, -rotdir(3, 1, :),  rotdir(2, 1, :) ; ...
+             rotdir(3, 1, :),                z, -rotdir(1, 1, :) ; ...
+            -rotdir(2, 1, :),  rotdir(1, 1, :),                z ];
+    else
+        error('Wrong input format. See function help.')
+    end
+end