Skip to content
Snippets Groups Projects
Commit d73eefad authored by Steve Tonneau's avatar Steve Tonneau Committed by Pierre Fernbach
Browse files

implemented binomial

parent d6d7d100
No related branches found
No related tags found
No related merge requests found
......@@ -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
......@@ -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");
......
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment