implemented binomial

include/centroidal-dynamics-lib/util.hh

@@ -7,6 +7,8 @@
 
 #include <iostream>
 #include <fstream>
+#include <cmath>
+#include <cassert>
 
 #include <Eigen/Dense>
 #include <Eigen/src/Core/util/Macros.h>
@@ -136,6 +138,63 @@ namespace centroidal_dynamics
 
   std::string getDateAndTimeAsString();
 
+  /**
+ * Computes a binomal coefficient
+ * @return  n!/((n–k)! k!).
+ */
+  value_type nchoosek(const int n, const int k);
+
+  template < typename DerivedV, typename DerivedU>
+  void doCombs(Eigen::Matrix<typename DerivedU::Scalar,1,Eigen::Dynamic>& running,
+               int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU> & U, const Eigen::MatrixBase<DerivedV> & V,
+               int offset, int k)
+  {
+      int N = (int)(V.size());
+      if(k==0)
+      {
+          U.row(running_i) = running;
+          running_i++;
+          return;
+      }
+      for (int i = offset; i <= N - k; ++i)
+      {
+          running(running_j) = V(i);
+          running_j++;
+          doCombs(running, running_i, running_j, U, V, i+1,k-1);
+          running_j--;
+      }
+  }
+
+  /**
+ * Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
+ * Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
+ * @param V  n-long vector of elements
+ * @param k  size of sub-set to consider
+ * @param U  result matrix
+ * @return nchoosek by k long matrix where each row is a unique k-size
+ * the first one, with all zeros.
+ */
+  template < typename DerivedV, typename DerivedU>
+  void nchoosek(
+    const Eigen::MatrixBase<DerivedV> & V,
+    const int k,
+    Eigen::PlainObjectBase<DerivedU> & U)
+    {
+        using namespace Eigen;
+        if(V.size() == 0)
+        {
+            U.resize(0,k);
+            return;
+        }
+        assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
+        U.resize(nchoosek((int)(V.size()),k),k);
+        int running_i  = 0;
+        int running_j = 0;
+        Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
+       /* const std::function<void(int,int)> doCombs =
+        [&running,&N,&doCombs,&running_i,&running_j,&U,&V](int offset, int k)*/;
+        doCombs(running, running_i, running_j, U, V,0,k);
+    }
 } //namespace centroidal_dynamics
 
 #endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH

src/centroidal_dynamics.cpp

@@ -521,12 +521,41 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
   SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
   return LP_STATUS_ERROR;
 }
+///
+/// \brief Computes factorial of a number
+///
+int fact(const int n)
+{
+    assert(n>=0);
+    int res = 1;
+    for (int i=2 ; i <= n ; ++i)
+       res *= i;
+    return res;
+}
+
+///
+/// \brief Computes a binomal coefficient
+///
+int choosenk(const int n, const int k)
+{
+    return fact(n) / (fact(k) * fact(n - k));
+}
 
 bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
 {
-    int n = (int)(v.rows());
-    int m = (int)(v.cols());
 
+    // todo: for the moment using ad hoc upper bound = 500 N
+    int n = (int)v.rows();
+    int m = (int)v.cols();
+    if (n>m)
+    {
+        SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
+        return false;
+    }
+    MatrixXX test = v;
+    VectorX vec;
+    nchoosek(vec,6,test);
+    //int I = I=nchoosek(1:m,n-1);*/
 //  getProfiler().start("eigen_to_cdd");
   dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
 //  getProfiler().stop("eigen_to_cdd");

src/util.cpp

@@ -165,6 +165,51 @@ std::string getDateAndTimeAsString()
   strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
   return std::string(buffer);
 }
+/*
+int fact(const int n)
+{
+    assert(n>=0);
+    int res = 1;
+    for (int i=2 ; i <= n ; ++i)
+       res *= i;
+    return res;
+}
+
+int choosenk(const int n, const int k)
+{
+    return fact(n) / (fact(k) * fact(n - k));
+}*/
+
+/* is this faster ?
+value_type choosenk(const int n, const int k)
+{
+    if(k>n/2)
+        return nchoosek(n,n-k);
+    else if(k==1)
+        return n;
+    else
+    {
+        double c = 1;
+        for(int i = 1;i<=k;i++)
+            c *= (((double)n-k+i)/((double)i));
+        return std::round(c);
+    }
+}*/
+
+value_type nchoosek(const int n, const int k)
+{
+    if(k>n/2)
+        return nchoosek(n,n-k);
+    else if(k==1)
+        return n;
+    else
+    {
+        value_type c = 1;
+        for(int i = 1;i<=k;i++)
+            c *= (((value_type)n-k+i)/((value_type)i));
+        return round(c);
+    }
+}
 
 } //namespace centroidal_dynamics