Pulling in Nathan's high-dimensional bugfixes

.gitignore

@@ -1,3 +1,4 @@
+
 # OS generated files #
 ######################
 .DS_Store

ProgramFiles/Utilities/LieGroups.m

@@ -20,15 +20,15 @@
         % matrix mapping
         function fn = mat_fn(group)
             if group == LieGroups.SE2
-                fn = @vec_to_mat_SE2;
+                fn = @vec_to_mat_SE2_lie;
             elseif group == LieGroups.SO3
                 fn = @vec_to_mat_SO3;    
             end
         end
         % vector mapping
         function fn = vec_fn(group)
             if group == LieGroups.SE2
-                fn = @mat_to_vec_SE2;
+                fn = @mat_to_vec_SE2_lie;
             elseif group == LieGroups.SO3
                 fn = @mat_to_vec_SO3;
             end

ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m

@@ -0,0 +1,6 @@
+function vector = mat_to_vec_SE2_lie(matrix)
+%Convert SE(2) matrices to a set of row vectors
+
+vector = [matrix(1,3);matrix(2,3);matrix(2,1)];
+
+end

ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m

@@ -0,0 +1,9 @@
+function matrix = vec_to_mat_SE2_lie(vector)
+
+theta = vector(3);
+x = vector(1);
+y = vector(2);
+
+matrix = [0,-theta,x;theta,0,y;0,0,0];
+
+end

ProgramFiles/sys_calcsystem_fcns/calc_ccf.m

@@ -36,8 +36,10 @@
         end
         % iter. thru J (all rows, col < row):
         entry = 1;
-        for r = 1:size(A,2)
-            for c = 1:r
+%         for r = 1:size(A,2)
+%             for c = 1:r
+        for c = 1:size(A,2)
+            for r = c:size(A,2)
                 % combine like bases (respecting wedge product rules)
                 % if doing a different choice of basis, change:
                     % which entry (r,c) pairs are saved in
@@ -47,7 +49,7 @@
                     continue;
                 end
                 % two partials for this basis
-                dA{i,entry} = -partials{r,c} + partials{c,r};
+                dA{i,entry} = partials{r,c} - partials{c,r};
 
                 entry = entry + 1;
             end
@@ -66,8 +68,10 @@
         lb_pts{i} = zeros(size(dA));
         % iterate thru. combinations of bases
         entry = 1;
-        for aj = 1:size(A,2)
-            for ak = 1:aj
+%         for aj = 1:size(A,2)
+%             for ak = 1:aj
+        for ak = 1:size(A,2)
+            for aj = ak:size(A,2)
                 % skip zero wedge products
                 if aj == ak
                     continue;
@@ -86,6 +90,7 @@
     %% combine for DA
     DA = cell(size(dA));
     for i = 1:numel(dA)
-        DA{i} = -(dA{i} + lb{i}); %sign choice for consistency
+        DA{i} = dA{i} - lb{i}; %sign choice for consistency
     end
+
 end