Add new helper functions for matrix operations (#881)

* Add new helper functions for matrix operations * Update Body/AAUHuman/BodyModels/GenericBodyModel/Helper.ClassTemplates.any Signed-off-by: Morten Enemark Lund <[email protected]> --------- Signed-off-by: Morten Enemark Lund <[email protected]>
AnyBody · Dec 12, 2023 · 1b37986 · 1b37986
1 parent a77a194
commit 1b37986
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 0 deletions.
diff --git a/Body/AAUHuman/BodyModels/GenericBodyModel/Helper.ClassTemplates.any b/Body/AAUHuman/BodyModels/GenericBodyModel/Helper.ClassTemplates.any
@@ -21,6 +21,21 @@
 
 #define REPLACE_ZEROS(x, replacement) ((x) + not(x)*(replacement))
 
+// Create a difference matrix of of size (N,N-1)
+// This can be used calcuate the difference between adjacent elements in a vector or matrix.
+#define DIFFMAT(N) (arrcat(zeros(1,N-1), eye(N-1)')' - arrcat(eye(N-1)', zeros(1,N-1))')
+
+// Pairwise distances between observations in n-dimensional space.
+#define PDIST(X) sum(vnorm(DIFFMAT(SizesOf(X)[0])*(X)))
+
+// Upper triangle of the array. Return a copy of an array with elements below the diagonal zeroed out 
+#define TRIU(IN) mult(IN, gteqfun(transpose(repmat(1,SizesOf(IN)[0] ,iarr(1,SizesOf(IN)[1]))), repmat(1,SizesOf(IN)[1],iarr(1,SizesOf(IN)[0]))))
+
+// Lower triangle of the array. Return a copy of an array with elements above the diagonal zeroed out 
+#define TRIL(IN) mult(IN, lteqfun(transpose(repmat(1,SizesOf(IN)[0] ,iarr(1,SizesOf(IN)[1]))), repmat(1,SizesOf(IN)[1],iarr(1,SizesOf(IN)[0]))))
+
+
+
 
 
 // Projection of P onto the line BA

diff --git a/Tests/test_helper_templates.any b/Tests/test_helper_templates.any
@@ -44,5 +44,62 @@ Main =
     AnyIntArray test_06 = assert(eqfun(NON_UNIQUE_POINTERS(P3), {&B}));
     AnyIntArray test_07 = assert(eqfun(NON_UNIQUE_POINTERS(P4), {&C}));
   };
+
+
+  AnyFolder Test_DIFFMAT = {
+    AnyFloat diffmat = DIFFMAT(5);
+    AnyInt test_01 = assert(eqfun(diffmat[1], {0,-1,1,0,0}));
+    AnyInt test_02 = assert(eqfun(diffmat[2], {0,0,-1,1,0})); 
+    AnyInt test_03 = assert(eqfun(SizesOf(diffmat),{4,5}));
+  };
+
+  AnyFolder Test_PDIST = {
+    AnyFloat Mat = {
+      {0,0,0},
+      {1,1,1},
+      {2,2,2},
+    };
+    AnyFloat length = PDIST(Mat);
+    AnyInt test = assert(eqfun(PDIST(Mat), 2*sqrt(3)));
+  };
+  AnyFolder Test_TRIU_TRIL = {
+
+   AnyFloat M_wide = ones(3,5);
+   AnyFloat M_tall = ones(6,3);
+   AnyInt test_TRIU_wide = assert(
+             eqfun(TRIU(M_wide),
+              {{1.0, 1.0, 1.0, 1.0, 1.0},
+               {0.0, 1.0, 1.0, 1.0, 1.0},
+               {0.0, 0.0, 1.0, 1.0, 1.0}}
+             )
+          );
+   AnyInt test_TRIU_tall = assert(
+             eqfun(TRIU(M_tall),
+              {{1.0, 1.0, 1.0},
+               {0.0, 1.0, 1.0},
+               {0.0, 0.0, 1.0},
+               {0.0, 0.0, 0.0},
+               {0.0, 0.0, 0.0}}
+             )
+          );
+   AnyInt test_TRIL_wide = assert(
+             eqfun(TRIL(M_wide),
+              {{1.0, 0.0, 0.0, 0.0, 0.0},
+               {1.0, 1.0, 0.0, 0.0, 0.0},
+               {1.0, 1.0, 1.0, 0.0, 0.0}}
+             )
+          );
+   AnyInt test_TRIL_tall = assert(
+             eqfun(TRIL(M_tall),
+              {{1.0, 0.0, 0.0},
+               {1.0, 1.0, 0.0},
+               {1.0, 1.0, 1.0},
+               {1.0, 1.0, 1.0},
+               {1.0, 1.0, 1.0}}
+             )
+          );
+  };
+
+
 };