Decrease number of concatenations required for dynamics with TrigPoly…

… states
Raptura · Jan 23, 2014 · 165d6fd · 165d6fd
1 parent e7028b7
commit 165d6fd
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 14 deletions.
diff --git a/thirdParty/spatial/Xrotz.m b/thirdParty/spatial/Xrotz.m
@@ -8,11 +8,15 @@
 
 c = cos(theta);
 s = sin(theta);
+% 
+% X = [  c  s  0  0  0  0 ;
+%       -s  c  0  0  0  0 ;
+%        0  0  1  0  0  0 ;
+%        0  0  0  c  s  0 ;
+%        0  0  0 -s  c  0 ;
+%        0  0  0  0  0  1
+%     ];
 
-X = [  c  s  0  0  0  0 ;
-      -s  c  0  0  0  0 ;
-       0  0  1  0  0  0 ;
-       0  0  0  c  s  0 ;
-       0  0  0 -s  c  0 ;
-       0  0  0  0  0  1
-    ];
+X = 0*theta(1)+eye(6);
+X_ind = sub2ind([6 6],[1,1,2,2,4,4,5,5],[1,2,1,2,4,5,4,5]);
+X(X_ind) = [c,s,-s,c,c,s,-s,c];
diff --git a/thirdParty/spatial/Xtrans.m b/thirdParty/spatial/Xtrans.m
@@ -5,10 +5,14 @@
 % coordinates for spatial motion vectors, in which frame B is translated by
 % an amount r (3D vector) relative to frame A.
 
-X = [  1     0     0    0  0  0 ;
-       0     1     0    0  0  0 ;
-       0     0     1    0  0  0 ;
-       0     r(3) -r(2) 1  0  0 ;
-      -r(3)  0     r(1) 0  1  0 ;
-       r(2) -r(1)  0    0  0  1
-    ];
+% X = [  1     0     0    0  0  0 ;
+%        0     1     0    0  0  0 ;
+%        0     0     1    0  0  0 ;
+%        0     r(3) -r(2) 1  0  0 ;
+%       -r(3)  0     r(1) 0  1  0 ;
+%        r(2) -r(1)  0    0  0  1
+%     ];
+
+X = 0*r(1)+eye(6);
+X_ind = sub2ind([6 6],[4,4,5,5,6,6],[2,3,1,3,1,2]);
+X(X_ind) = [r(3),-r(2),-r(3),r(1),r(2),-r(1)];